int main(int argc, char *args[]) { char buffer[255]; // Buffer for sending data int i; // Index if (argc == 4) { // Get parameters offset = atoi(args[1]); migrx = atof(args[2]); migrz = atof(args[3]); //migration goal point comes from the controller arguments. It is defined in the world-file, under "controllerArgs" of the supervisor. printf("Migratory instinct : (%f, %f)\n", migrx, migrz); } else { printf("Missing argument\n"); return 1; } orient_migr = -atan2f(migrx,migrz); if (orient_migr<0) { orient_migr+=2*M_PI; // Keep value within 0, 2pi } reset(); send_init_poses(); // Compute reference fitness values float fit_cluster; // Performance metric for aggregation float fit_orient; // Performance metric for orientation for(;;) { wb_robot_step(TIME_STEP); if (t % 10 == 0) { for (i=0;i<FLOCK_SIZE;i++) { // Get data loc[i][0] = wb_supervisor_field_get_sf_vec3f(robs_trans[i])[0]; // X loc[i][1] = wb_supervisor_field_get_sf_vec3f(robs_trans[i])[2]; // Z loc[i][2] = wb_supervisor_field_get_sf_rotation(robs_rotation[i])[3]; // THETA // Sending positions to the robots, comment the following two lines if you don't want the supervisor sending it sprintf(buffer,"%1d#%f#%f#%f##%f#%f",i+offset,loc[i][0],loc[i][1],loc[i][2], migrx, migrz); wb_emitter_send(emitter,buffer,strlen(buffer)); } //Compute and normalize fitness values compute_fitness(&fit_cluster, &fit_orient); fit_cluster = fit_cluster_ref/fit_cluster; fit_orient = 1-fit_orient/M_PI; printf("time:%d, Topology Performance: %f\n", t, fit_cluster); } t += TIME_STEP; } }
void send_init_poses(void) { char buffer[255]; // Buffer for sending data int i; for (i=0;i<FLOCK_SIZE;i++) { // Get data loc[i][0] = wb_supervisor_field_get_sf_vec3f(robs_trans[i])[0]; // X loc[i][1] = wb_supervisor_field_get_sf_vec3f(robs_trans[i])[2]; // Z loc[i][2] = wb_supervisor_field_get_sf_rotation(robs_rotation[i])[3]; // THETA printf("Supervisor %d %d %d/n",loc[i][0],loc[i][0],loc[i][0]); // Send it out sprintf(buffer,"%1d#%f#%f#%f##%f#%f",i+offset,loc[i][0],loc[i][1],loc[i][2], migrx, migrz); wb_emitter_send(emitter,buffer,strlen(buffer)); // Run one step wb_robot_step(TIME_STEP); } }
// main loop int main(void) { srand(time(NULL)); // initialization wb_robot_init(); int i; for (i=0;i<ROBOTS;i++) { char aux[15]; sprintf(aux,"%s%d",rob_prefix,i+1); rob[i] = wb_supervisor_node_get_from_def(aux); loc[i] = wb_supervisor_field_get_sf_vec3f(wb_supervisor_node_get_field(rob[i],"translation")); initLoc[i][0] = loc[i][0]; initLoc[i][1] = loc[i][1]; initLoc[i][2] = loc[i][2]; rot[i] = wb_supervisor_field_get_sf_rotation(wb_supervisor_node_get_field(rob[i],"rotation")); initRot[i][0] = rot[i][0]; initRot[i][1] = rot[i][1]; initRot[i][2] = rot[i][2]; initRot[i][3] = rot[i][3]; } reset(); wb_robot_step(2*STEP_SIZE); // start the controller outfile = fopen("../../../matlab/output.m","w"); printf("Starting main loop...\n"); while (wb_robot_step(STEP_SIZE) != -1) { run(STEP_SIZE); } wb_robot_cleanup(); return 0; }
// this is what is done at every time step independently of the game state static void step() { // copy pointer to ball position values if (ball_translation) ball_pos = wb_supervisor_field_get_sf_vec3f(ball_translation); int i; for (i = 0; i < NUM_ROBOTS; i++) { // copy pointer to robot position values if (robot_translation[i]) robot_pos[i] = wb_supervisor_field_get_sf_vec3f(robot_translation[i]); if (robot_rotation[i]) robot_rot[i] = wb_supervisor_field_get_sf_rotation(robot_rotation[i]); } if (message_steps) message_steps--; // yield control to simulator wb_robot_step(TIME_STEP); // every 480 milliseconds if (step_count++ % 12 == 0) sendGameControlData(); //checkFalling(); // did I receive a message ? //read_incoming_messages(); // read key pressed check_keyboard(); }
int main(int argc, char *args[]) { int i; // Index reset(); // initialize fitness values float fit_O; float fit_C; float fit_V; float fit_P; float fit_S; float perf_sum = 0.0f; int nb_measur = 0; //number of measurment of instant perf for(;;) { wb_robot_step(TIME_STEP); if (t % 100 == 0) { for (i=0;i<FLOCK_SIZE;i++) { // initialize old position loc_old[i][0] = loc[i][0]; loc_old[i][1] = loc[i][1]; loc_old[i][2] = loc[i][2]; // initialize current position loc[i][0] = wb_supervisor_field_get_sf_vec3f(robs_trans[i])[0]; loc[i][1] = wb_supervisor_field_get_sf_vec3f(robs_trans[i])[2]; loc[i][2] = wb_supervisor_field_get_sf_rotation(robs_rotation[i])[3]; } // compute the orientation metric compute_fitness_O(& fit_O); // compute the cohesion metric compute_fitness_C(& fit_C); // compute the velocity metric compute_fitness_V(& fit_V); // compute entropy metric compute_fitness_S(& fit_S); // compute total metric value fit_P = instant_perf(); // Display fitness printf("time : %d, orientation , cohesion , velocity , entropy, instant perf : %.4lf, %.4lf, %.4lf, %.4lf, %.4lf\n", t, fit_O, fit_C, fit_V, fit_S, fit_P); //avoid wrong values if(fit_P > 0){ perf_sum += fit_P; nb_measur++; } } if (t % 1000 == 0){ printf("time : %d, overall perf : %.4lf\n", t, perf_sum/nb_measur); } t += TIME_STEP; } }
int main() { printf("hello from supervisor\n"); const char *robot_name[ROBOTS] = {"NAO"}; WbNodeRef node; WbFieldRef robot_translation_field[ROBOTS],robot_rotation_field[ROBOTS],ball_translation_field; //WbDeviceTag emitter, receiver; int i,j; int score[2] = { 0, 0 }; double time = 10 * 60; /* a match lasts for 10 minutes */ double ball_reset_timer = 0; double ball_initial_translation[3] = { -2.5, 0.0324568, 0 }; double robot_initial_translation[ROBOTS][3] = { {-4.49515, 0.234045, -0.0112415}, {0.000574037, 0.332859, -0.00000133636}}; double robot_initial_rotation[ROBOTS][4] = { {0.0604202, 0.996035, -0.0652942, 1.55047}, {0.000568956, 0.70711, 0.707104, 3.14045}}; double packet[ROBOTS * 3 + 2]; char time_string[64]; const double *robot_translation[ROBOTS], *robot_rotation[ROBOTS], *ball_translation; wb_robot_init(); time_step = wb_robot_get_basic_time_step(); emitter = wb_robot_get_device("emitter"); wb_receiver_enable(emitter, time_step); receiver = wb_robot_get_device("receiver"); wb_receiver_enable(receiver, time_step); for (i = 0; i < ROBOTS; i++) { node = wb_supervisor_node_get_from_def(robot_name[i]); robot_translation_field[i] = wb_supervisor_node_get_field(node,"translation"); robot_translation[i] = wb_supervisor_field_get_sf_vec3f(robot_translation_field[i]); for(j=0;j<3;j++) robot_initial_translation[i][j]=robot_translation[i][j]; robot_rotation_field[i] = wb_supervisor_node_get_field(node,"rotation"); robot_rotation[i] = wb_supervisor_field_get_sf_rotation(robot_rotation_field[i]); for(j=0;j<4;j++) robot_initial_rotation[i][j]=robot_rotation[i][j]; } node = wb_supervisor_node_get_from_def("BALL"); ball_translation_field = wb_supervisor_node_get_field(node,"translation"); ball_translation = wb_supervisor_field_get_sf_vec3f(ball_translation_field); for(j=0;j<3;j++) ball_initial_translation[j]=ball_translation[j]; /* printf("ball initial translation = %g %g %g\n",ball_translation[0],ball_translation[1],ball_translation[2]); */ set_scores(0, 0); while(wb_robot_step(TIME_STEP)!=-1) { //printf("supervisor commands START!\n"); check_for_slaves_data(); ball_translation = wb_supervisor_field_get_sf_vec3f(ball_translation_field); for (i = 0; i < ROBOTS; i++) { robot_translation[i]=wb_supervisor_field_get_sf_vec3f(robot_translation_field[i]); /* printf("coords for robot %d: %g %g %g\n",i,robot_translation[i][0],robot_translation[i][1],robot_translation[i][2]); */ packet[3 * i] = robot_translation[i][0]; /* robot i: X */ packet[3 * i + 1] = robot_translation[i][2]; /* robot i: Z */ if (robot_rotation[i][1] > 0) { /* robot i: rotation Ry axis */ packet[3 * i + 2] = robot_rotation[i][3]; /* robot i: alpha */ } else { /* Ry axis was inverted */ packet[3 * i + 2] = -robot_rotation[i][3]; } } packet[3 * ROBOTS] = ball_translation[0]; /* ball X */ packet[3 * ROBOTS + 1] = ball_translation[2]; /* ball Z */ wb_emitter_send(emitter, packet, sizeof(packet)); /* Adds TIME_STEP ms to the time */ time -= (double) TIME_STEP / 1000; if (time < 0) { time = 10 * 60; /* restart */ } sprintf(time_string, "%02d:%02d", (int) (time / 60), (int) time % 60); wb_supervisor_set_label(2, time_string, 0.45, 0.01, 0.1, 0x000000, 0.0); /* black */ if (ball_reset_timer == 0) { if (ball_translation[0] > GOAL_X_LIMIT) { /* ball in the blue goal */ set_scores(++score[0], score[1]); ball_reset_timer = 3; /* wait for 3 seconds before reseting the ball */ } else if (ball_translation[0] < -GOAL_X_LIMIT) { /* ball in the yellow goal */ set_scores(score[0], ++score[1]); ball_reset_timer = 3; /* wait for 3 seconds before reseting the ball */ } } else { ball_reset_timer -= (double) TIME_STEP / 1000.0; if (ball_reset_timer <= 0) { ball_reset_timer = 0; wb_supervisor_field_set_sf_vec3f(ball_translation_field, ball_initial_translation); for (i = 0; i < ROBOTS; i++) { wb_supervisor_field_set_sf_vec3f(robot_translation_field[i], robot_initial_translation[i]); wb_supervisor_field_set_sf_rotation(robot_rotation_field[i], robot_initial_rotation[i]); } } } } wb_robot_cleanup(); return 0; }