void reset(void) { int i; char stringaux[20]; //get e-pucks node names and location field node names for(i=0;i<NUM_ROBOTS;i++){ sprintf(stringaux,"E_PUCK_%d",i+1); epucks[i]=wb_supervisor_node_get_from_def(stringaux); locfield[i]=wb_supervisor_node_get_field(epucks[i],"translation"); } //get beacon node name and location field node names beacon=wb_supervisor_node_get_from_def("BEACON1"); beacon_location=wb_supervisor_node_get_field(beacon,"translation"); srand(time(NULL)); //initialize random position for e-puck and beacon change_robot_positions(); move_beacon_random(); //initialize finaltime and time for first beacon repositioning finaltime=wb_robot_get_time()+EXP_TIME; // 1000 == 16min40sec beacon_timer=BEACON_TIMER; //60 == 1min }
/* * Initialize flock position and devices */ void reset(void) { wb_robot_init(); char rob[7] = "epuck0"; int i; for (i=0;i<FLOCK_SIZE;i++) { sprintf(rob,"epuck%d",i+offset); robs[i] = wb_supervisor_node_get_from_def(rob); robs_trans[i] = wb_supervisor_node_get_field(robs[i],"translation"); robs_rotation[i] = wb_supervisor_node_get_field(robs[i],"rotation"); } }
void change_robot_positions(){ int i; WbFieldRef rotfield; int location[2]; int rotation; double newlocation[3]={0.0, 0.0, 0.0}; //3D vector (x,z,y) double newrotation[4]={0.0, 1.0, 0.0, 0.0}; //3D unit vector (rotation axis), angle (rad) for(i=0;i<NUM_ROBOTS;i++){ //randomize e-puck location inside the arena location[0]=(rand()%(ARENASIDE-10))-ARENASIDE/2+5; location[1]=(rand()%(ARENASIDE-10))-ARENASIDE/2+5; newlocation[0]=((double)location[0])/100; newlocation[1]=0.0; newlocation[2]=((double)location[1])/100; wb_supervisor_field_set_sf_vec3f(locfield[i],newlocation); //randomize e-puck rotation (steps of 0.0628rad) rotation=rand()%100; newrotation[3]=(double)rotation/100*2*M_PI; rotfield=wb_supervisor_node_get_field(epucks[i],"rotation"); wb_supervisor_field_set_sf_rotation(rotfield,newrotation); } }
int main(int argc, char *argv[]) { wb_robot_init(); WbNodeRef robot_node = wb_supervisor_node_get_from_def("rob0"); WbFieldRef trans_field = wb_supervisor_node_get_field(robot_node, "translation"); double count = 0; while(1) { char time[10]; sprintf(time,"%f sec",count); wb_supervisor_set_label(0,time,0,0,0.1,0xff0000,0); const double *trans = wb_supervisor_field_get_sf_vec3f(trans_field); //printf("MY_ROBOT is at position: %g %g %g\n", trans[0], trans[1], trans[2]); if (x==1 && trans[0]<-1.7 && trans[2]<-0.65) { wb_supervisor_export_image ("/home/afroze/Desktop/photo.jpg",50); printf("\n\n\n\nPHOTOOOOOO\n\n\n\n\n"); return 0; } wb_robot_step(TIME_STEP); count+=0.064; } return 0; }
void randomize_event_color(int event_index) { char s[50]; char *eventprefix = (char *) "e"; double* color = red; int color_select = (int)(3*rnd()); switch (color_select) { case 0: color=red; break; case 1: color=green; break; case 2: color=blue; break; } sprintf(s, "%s%d_material", eventprefix, event_index); WbNodeRef eventAppearance = wb_supervisor_node_get_from_def(s); WbFieldRef eventColor = wb_supervisor_node_get_field(eventAppearance,"diffuseColor"); wb_supervisor_field_set_sf_color(eventColor, color); eventColor = wb_supervisor_node_get_field(eventAppearance,"emissiveColor"); wb_supervisor_field_set_sf_color(eventColor, color); }
/* Reset the supervisor */ void reset(void) { int i; char s[50]; char *eventprefix = (char *) "e"; printed = false; for (i=0;i<ROBOTS;i++) { char aux[15]; /* Get and save a reference to the robot. */ sprintf(aux,"%s%d",rob_prefix,i+1); // rob_name << rob_prefix << i+1; // << "_0"; rob[i] = wb_supervisor_node_get_from_def(aux); wb_supervisor_field_set_sf_vec3f(wb_supervisor_node_get_field(rob[i],"translation"), initLoc[i]); wb_supervisor_field_set_sf_rotation(wb_supervisor_node_get_field(rob[i],"rotation"), initRot[i]); robTrans[i] = wb_supervisor_node_get_field(rob[i],"translation"); previous_x[i] = wb_supervisor_field_get_sf_vec3f(robTrans[i])[0]; previous_y[i] = wb_supervisor_field_get_sf_vec3f(robTrans[i])[2]; /* Get robot emitters */ strcpy(aux,emi_prefix); sprintf(aux,"%s%d",aux,i+1); } printf("Initializing events...\n"); for (i=0; i<MAX_EVENTS; i++) { sprintf(s, "%s%d", eventprefix, i); // Define position of event and place it there events[i].event = wb_supervisor_node_get_from_def(s); events[i].eventTrans = wb_supervisor_node_get_field(events[i].event,"translation"); events[i].state = 1.0; randomize_event_position(i); randomize_event_color(i); } }
// 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; }
void randomize_event_position(int event_index) { char s[50]; char *eventprefix = (char *) "e"; double pos[3]; sprintf(s, "%s%d", eventprefix, event_index); //wb_supervisor_node_get_from_def(s); // Define position of event and place it there pos[0] = ARENA_SIZE*rnd()-ARENA_SIZE/2.0; pos[1] = 0.01; pos[2] = ARENA_SIZE*rnd()-ARENA_SIZE/2.0; wb_supervisor_field_set_sf_vec3f(wb_supervisor_node_get_field(wb_supervisor_node_get_from_def(s),"translation"), pos); }
int main() { wb_robot_init(); // do this once only WbNodeRef robot_node1 = wb_supervisor_node_get_from_def("epuck1"); WbFieldRef trans_field1 = wb_supervisor_node_get_field(robot_node1, "translation"); /*WbNodeRef robot_node2 = wb_supervisor_node_get_from_def("epuck2"); WbFieldRef trans_field2 = wb_supervisor_node_get_field(robot_node2, "translation"); WbNodeRef robot_node3 = wb_supervisor_node_get_from_def("epuck3"); WbFieldRef trans_field3 = wb_supervisor_node_get_field(robot_node3, "translation"); WbNodeRef robot_node4 = wb_supervisor_node_get_from_def("epuck4"); WbFieldRef trans_field4 = wb_supervisor_node_get_field(robot_node4, "translation"); WbNodeRef robot_node5 = wb_supervisor_node_get_from_def("epuck5"); WbFieldRef trans_field5 = wb_supervisor_node_get_field(robot_node5, "translation"); WbNodeRef robot_node6 = wb_supervisor_node_get_from_def("epuck6"); WbFieldRef trans_field6 = wb_supervisor_node_get_field(robot_node6, "translation"); WbNodeRef robot_node7 = wb_supervisor_node_get_from_def("epuck7"); WbFieldRef trans_field7 = wb_supervisor_node_get_field(robot_node7, "translation"); */ time_t now; struct tm *today; char date[23]; //get current date time(&now); today = localtime(&now); //print it in DD.MM.YY format. strftime(date, 23, "sim%Y%m%d.%H%M%S.txt", today); FILE *fp; fp=fopen(date, "w"); double time = 0.0; for (time = 0.0; time < 3600.0; time += TIME_STEP / 1000.0) { // this is done repeatedly const double *trans1 = wb_supervisor_field_get_sf_vec3f(trans_field1); fprintf(fp, "%g,%g",trans1[0], trans1[2]); /*const double *trans2 = wb_supervisor_field_get_sf_vec3f(trans_field2); fprintf(fp, ",%g,%g",trans2[0], trans2[2]); const double *trans3 = wb_supervisor_field_get_sf_vec3f(trans_field3); fprintf(fp, ",%g,%g",trans3[0], trans3[2]); const double *trans4 = wb_supervisor_field_get_sf_vec3f(trans_field4); fprintf(fp, ",%g,%g",trans4[0], trans4[2]); const double *trans5 = wb_supervisor_field_get_sf_vec3f(trans_field5); fprintf(fp, ",%g,%g",trans5[0], trans5[2]); const double *trans6 = wb_supervisor_field_get_sf_vec3f(trans_field6); fprintf(fp, ",%g,%g",trans6[0], trans6[2]); const double *trans7 = wb_supervisor_field_get_sf_vec3f(trans_field7); fprintf(fp, ",%g,%g",trans7[0], trans7[2]);*/ fprintf(fp, "\n"); wb_robot_step(TIME_STEP); } wb_supervisor_simulation_quit(EXIT_SUCCESS); fclose(fp); wb_robot_cleanup(); wb_robot_step(TIME_STEP); wb_robot_step(TIME_STEP); wb_robot_step(TIME_STEP); return 0; }
/* * The call to wb_robot_step is mandatory for the function * supervisor_node_was_found to be able to work correctly. */ static void reset(void) { int i; for(i=0; i<POP_SIZE; i++) fitness[i]=-1; srand(time(0)); pF1= fopen("initPop.txt","w+"); //pF2= fopen("real2IntGenome.txt","w+"); //pF3= fopen("encodedPop.txt","w+"); // Initiate the emitter used to send genomes to the experiment emitter = wb_robot_get_device("emittersupervisor"); // Initiate the receiver used to receive fitness receiver = wb_robot_get_device("receiversupervisor"); wb_receiver_enable(receiver, TIME_STEP); // Create a supervised node for the robot robot = wb_supervisor_node_get_from_def("EPUCK"); assert(robot!=NULL); pattern=wb_supervisor_node_get_from_def("FLOOR"); pattern_rotation = wb_supervisor_node_get_field(pattern,"rotation"); assert(pattern!=NULL); assert(pattern_rotation!=NULL); trans_field = wb_supervisor_node_get_field(robot,"translation"); rot_field = wb_supervisor_node_get_field(robot,"rotation"); ctrl_field= wb_supervisor_node_get_field(robot,"controller"); wb_robot_step(0); wb_robot_step(0); //this is magic // set the robot controller to nn const char *controller_name = "drive2"; wb_supervisor_field_set_sf_string(ctrl_field,controller_name); //check whether robot was found if (wb_supervisor_node_get_type(robot) == WB_NODE_NO_NODE) puts("Error: node EPUCK not found!!!"); if(EVOLVING) { //Open log files f1= fopen ("../../data/fitness.txt", "wt"); f2= fopen ("../../data/genomes.txt", "wt"); //pR= fopen("savePop.txt","w+"); //initial weights randomly initializePopulation(); //rtoi(); //real to int conversion before encoding popEncoder(); //puts("NEW EVOLUTION"); //puts("GENERATION 0"); // send genomes to experiment resetRobotPosition(); wb_emitter_send(emitter, (void *)pop_bin[evaluated_inds], GENOME_LENGTH*sizeof(_Bool)); //instead save binary genomes to a file //puts("Genes sent."); } else { // testing best individual // Read best genome from bestgenome.txt and initialize weights. f3= fopen ("../../data/bestgenome.txt", "rt"); fscanf(f3,"%d %d", &generation, &evaluated_inds); //TODO either read binary genome or make sure it is decoded prior to storage for(i=0;i<GENOME_LENGTH;i++) fscanf(f3,"%d ",&pop_bin[0][i]); //printf("TESTING INDIVIDUAL %d, GENERATION %d\n", evaluated_inds, generation); // send genomes to experiment resetRobotPosition(); // wb_emitter_send(emitter, (void *)pop[0], NB_GENES*sizeof(double)); } return; }
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; }
/* * This is the main program. * The arguments of the main function can be specified by the * "controllerArgs" field of the Robot node */ int main(int argc, char **argv) { //int cont=0; using_shared_memory(); /* necessary to initialize webots stuff */ wb_robot_init(); // do this once only Darwin WbNodeRef robot_node = wb_supervisor_node_get_from_def("Darwin"); WbFieldRef trans_field = wb_supervisor_node_get_field(robot_node, "translation"); WbFieldRef rot_field = wb_supervisor_node_get_field(robot_node, "rotation"); int caiu_cont = 0; // do this once only Darwin2 WbNodeRef robot_node_2 = wb_supervisor_node_get_from_def("Darwin2"); WbFieldRef trans_field_2 = wb_supervisor_node_get_field(robot_node_2, "translation"); WbFieldRef rot_field_2 = wb_supervisor_node_get_field(robot_node_2, "rotation"); // do this once only Darwin3 WbNodeRef robot_node_3 = wb_supervisor_node_get_from_def("Darwin3"); WbFieldRef trans_field_3 = wb_supervisor_node_get_field(robot_node_3, "translation"); WbFieldRef rot_field_3 = wb_supervisor_node_get_field(robot_node_3, "rotation"); while (1) { // this is done repeatedly const double *trans = wb_supervisor_field_get_sf_vec3f(trans_field); const double *trans_2 = wb_supervisor_field_get_sf_vec3f(trans_field_2); const double *trans_3 = wb_supervisor_field_get_sf_vec3f(trans_field_3); TRANS1 = trans[0]; TRANS2 = trans[1]; TRANS3 = trans[2]; TRANS1_2 = trans_2[0]; TRANS2_2 = trans_2[1]; TRANS3_2 = trans_2[2]; TRANS1_3 = trans_3[0]; TRANS2_3 = trans_3[1]; TRANS3_3 = trans_3[2]; //printf("MY_ROBOT is at position: %g %g %g\n", trans[0], trans[1], trans[2]); wb_robot_step(32); if(RESET_ROBOT == 1) { caiu_cont++; CAIU_CONT = caiu_cont; // reset robot position const double INITIAL[3] = { 0, 0.32004, 0 }; const double INITIAL_ROT[4] = { 0.211189, 0.971678, -0.106025, 0.944968 }; wb_supervisor_field_set_sf_vec3f(trans_field, INITIAL); wb_supervisor_field_set_sf_rotation(rot_field, INITIAL_ROT); //wb_supervisor_simulation_reset_physics(); RESET_ROBOT = 0; if(caiu_cont > 25) { caiu_cont = 0; wb_supervisor_simulation_revert(); // restart the simulation // A medida que o tempo vai passando o robo Darwin vai ficando // cada vez mais devagar como se houvesse algum tipo de desgaste fisico //entao e necessario restarta a simulaçao } } if(RESET_ROBOT_2 == 1) { // reset robot position const double INITIAL_2[3] = { 0.2, 0.32004, 4.7 }; const double INITIAL_ROT_2[4] = { 0.211189, 0.971678, -0.106025, 0.944968 }; wb_supervisor_field_set_sf_vec3f(trans_field_2, INITIAL_2); wb_supervisor_field_set_sf_rotation(rot_field_2, INITIAL_ROT_2); //wb_supervisor_simulation_reset_physics(); RESET_ROBOT_2 = 0; } if(RESET_ROBOT_3 == 1) { // reset robot position const double INITIAL_3[3] = { 3.2, 0.32004, -3.7 }; const double INITIAL_ROT_3[4] = { 0.211189, 0.971678, -0.106025, 0.944968 }; wb_supervisor_field_set_sf_vec3f(trans_field_3, INITIAL_3); wb_supervisor_field_set_sf_rotation(rot_field_3, INITIAL_ROT_3); //wb_supervisor_simulation_reset_physics(); RESET_ROBOT_3 = 0; } } /* * You should declare here WbDeviceTag variables for storing * robot devices like this: * WbDeviceTag my_sensor = wb_robot_get_device("my_sensor"); * WbDeviceTag my_actuator = wb_robot_get_device("my_actuator"); */ /* This is necessary to cleanup webots resources */ wb_robot_cleanup(); return 0; }
// initialize devices and data static void initialize() { // necessary to initialize Webots wb_robot_init(); // initialize game control data memset(&control_data, 0, sizeof(control_data)); memcpy(control_data.header, GAMECONTROLLER_STRUCT_HEADER, sizeof(GAMECONTROLLER_STRUCT_HEADER)); control_data.version = GAMECONTROLLER_STRUCT_VERSION; control_data.playersPerTeam = NUM_ROBOTS / 2; control_data.state = STATE_INITIAL; control_data.secondaryState = STATE2_NORMAL; control_data.teams[0].teamColour = TEAM_BLUE; control_data.teams[1].teamColour = TEAM_RED; // emitter for sending game control data and receiving 'move' requests //emitter = wb_robot_get_device("emitter"); //receiver = wb_robot_get_device("receiver"); //wb_receiver_enable(receiver, TIME_STEP); // get robot field tags for getting/setting their positions int i; for(i = 0; i < NUM_ROBOTS; i++) { WbNodeRef robot = wb_supervisor_node_get_from_def(ROBOT_DEF_NAMES[i]); if (robot) { robot_translation[i] = wb_supervisor_node_get_field(robot, "translation"); robot_rotation[i] = wb_supervisor_node_get_field(robot, "rotation"); robot_controller[i] = wb_supervisor_node_get_field(robot, "controller"); } else { robot_translation[i] = NULL; robot_rotation[i] = NULL; robot_controller[i] = NULL; } } // to keep track of ball position WbNodeRef ball = wb_supervisor_node_get_from_def("BALL"); if (ball) ball_translation = wb_supervisor_node_get_field(ball, "translation"); // eventually read teams names from file FILE *file = fopen("teams.txt", "r"); if (file) { fscanf(file, "%[^\n]\n%[^\n]", team_names[TEAM_BLUE], team_names[TEAM_RED]); fclose(file); } // variable set during official matches const char *WEBOTS_ROBOTSTADIUM = getenv("WEBOTS_ROBOTSTADIUM"); if (WEBOTS_ROBOTSTADIUM) { if (strcmp(WEBOTS_ROBOTSTADIUM, "ROUND") == 0) { match_type = ROUND; printf("Running Robotstadium ROUND match\n"); } else if (strcmp(WEBOTS_ROBOTSTADIUM, "FINAL") == 0) { match_type = FINAL; printf("Running Robotstadium FINAL match\n"); } } // make video if (match_type != DEMO) { // format=640x480, type=MPEG4, quality=75% wb_supervisor_start_movie("movie.avi", 640, 480, 0, 75); } // enable keyboard for manual score control wb_robot_keyboard_enable(TIME_STEP * 10); }