int main() { wb_robot_init(); printf("hello from NAO\n"); time_step = wb_robot_get_basic_time_step(); receiver = wb_robot_get_device("receiver"); wb_receiver_enable(receiver, time_step); emitter = wb_robot_get_device("emitter2"); wb_receiver_enable(emitter, time_step); find_and_enable_devices(); wb_motor_set_position (RShoulderPitch, 1.57079633); wb_motor_set_position (LShoulderPitch, 1.57079633); wb_motor_set_position (HeadYaw, 0.0); //wb_motor_set_position (HeadPitch, 0.1); //10 degrees // run until simulation is restarted while (wb_robot_step(time_step) != -1) { //check_for_new_genes(); //sense_compute_and_actuate(); //int samples = wb_camera_get_width(LaserHead); //double field_of_view = wb_camera_get_fov(LaserHead); //const float *values = wb_camera_get_range_image(LaserHead); report_step_state_to_supervisor(); //printf("%f\n", field_of_view); //printf("size: %d\n", sizeof(values)); //int i; //for(i=0; i<samples;i++){ // printf("value %d is %f\n",i, values[i]); //} if(wb_robot_step(time_step) == 0){ //report_step_state_to_supervisor(); } } wb_robot_cleanup(); return 0; }
/* * Reset the robot's devices and get its ID */ static void reset() { int i; wb_robot_init(); char s[4]="ps0"; for(i=0; i<NB_SENSORS;i++) { ds[i]=wb_robot_get_device(s); // the device name is specified in the world file s[2]++; // increases the device number } for(i=0; i<FLOCK_SIZE; i++) { timestamp[i] = 0; relative_pos[i][0] = 0; relative_pos[i][1] = 0; relative_pos[i][2] = 0; } maxTimestamp = 1; robot_name=(char*) wb_robot_get_name(); for(i=0;i<NB_SENSORS;i++) wb_distance_sensor_enable(ds[i],64); my_position[0] = 0; my_position[1] = 0; my_position[2] = 0; //printf("reset: my pos: %f, %f\n", my_position[0], my_position[1]); wb_differential_wheels_enable_encoders(64); //Reading the robot's name. Pay attention to name specification when adding robots to the simulation! sscanf(robot_name,"epuck%d",&robot_id_u); // read robot id from the robot's name robot_id = robot_id_u%FLOCK_SIZE; // normalize between 0 and FLOCK_SIZE-1 sprintf(robot_number, "%d", robot_id_u); receiver = wb_robot_get_device("receiver"); receiver0 = wb_robot_get_device("receiver0"); emitter = wb_robot_get_device("emitter"); emitter0 = wb_robot_get_device("emitter0"); wb_receiver_enable(receiver,32); wb_receiver_enable(receiver0,32); for(i=0; i<FLOCK_SIZE; i++) { initialized[i] = 0; // Set initialization to 0 (= not yet initialized) } }
/* * Reset the robot's devices and get its ID */ static void reset() { wb_robot_init(); receiver = wb_robot_get_device("receiver"); emitter = wb_robot_get_device("emitter"); compass2 = wb_robot_get_device("compass2"); receiver0 = wb_robot_get_device("receiver0"); emitter0 = wb_robot_get_device("emitter0"); int i; char s[4]="ps0"; for(i=0; i<NB_SENSORS;i++) { ds[i]=wb_robot_get_device(s); // the device name is specified in the world file s[2]++; // increases the device number } robot_name=(char*) wb_robot_get_name(); for(i=0; i<FLOCK_SIZE; i++) { relative_pos[i][0] = my_position[0] = 0; relative_pos[i][1] = my_position[1] = 0; relative_pos[i][2] = my_position[2] = 0; } for(i=0;i<NB_SENSORS;i++) wb_distance_sensor_enable(ds[i],64); wb_receiver_enable(receiver,64); wb_receiver_enable(receiver0,64); wb_compass_enable(compass2, 64); //Reading the robot's name. Pay attention to name specification when adding robots to the simulation! sscanf(robot_name,"epuck%d",&robot_id_u); // read robot id from the robot's name robot_id = robot_id_u%FLOCK_SIZE; // normalize between 0 and FLOCK_SIZE-1 for(i=0; i<FLOCK_SIZE; i++) { initialized[i] = 0; // Set initialization to 0 (= not yet initialized) } }
// // Reset the robot controller and initiate the sensors and emitter/receiver // static int reset() { int i; mode =1; emitter = wb_robot_get_device("emitter"); //buffer = (double *) emitter_get_buffer(emitter); receiver = wb_robot_get_device("receiver"); wb_receiver_enable(receiver, TIME_STEP); char text[5]="led0"; for(i=0;i<NB_LEDS;i++) { led[i]=wb_robot_get_device(text); // get a handler to the sensor text[3]++; // increase the device name to "ps1", "ps2", etc. } text[0]='p'; text[1]='s'; text[3]='\0'; text[2]='0'; ps[0] = wb_robot_get_device(text); // proximity sensors text[2]='7'; ps[1] = wb_robot_get_device(text); // proximity sensors text[2]='1'; ps[2] = wb_robot_get_device(text); // proximity sensors text[2]='6'; ps[3] = wb_robot_get_device(text); // proximity sensors text[2]='2'; ps[4] = wb_robot_get_device(text); // proximity sensors text[2]='5'; ps[5] = wb_robot_get_device(text); // proximity sensors text[2]='3'; ps[6] = wb_robot_get_device(text); // proximity sensors text[2]='4'; ps[7] = wb_robot_get_device(text); // proximity sensors // Enable proximity and floor sensors for(i=0;i<NB_DIST_SENS;i++) { wb_distance_sensor_enable(ps[i],TIME_STEP); printf("ps[%d] is active\n",i); } // Enable GPS sensor to determine position gps=wb_robot_get_device("gps"); wb_gps_enable(gps, TIME_STEP); printf("gps is active\n"); return 1; }
int main(int argc, char *argv[]) { /* define variables */ WbDeviceTag turret_sensor; double perf_data[3]; double clock = 0; /* initialize Webots */ wb_robot_init(); // read robot id from the robot's name char* robot_name; robot_name=(char*) wb_robot_get_name(); sscanf(robot_name,"e-puck%d",&robot_id); emitter = wb_robot_get_device("emitter"); wb_emitter_set_range(emitter,COM_RANGE); receiver = wb_robot_get_device("receiver"); wb_receiver_set_channel(receiver,0); wb_receiver_enable(receiver,TIME_STEP); wb_robot_step(TIME_STEP); turret_sensor = wb_robot_get_device("ts"); wb_light_sensor_enable(turret_sensor,TIME_STEP); /* main loop */ while (wb_robot_step(TIME_STEP) != -1) { clock += (double)TIME_STEP/1000; /* get light sensor value */ sensor_value = wb_light_sensor_get_value(turret_sensor); if(wb_receiver_get_queue_length(receiver) > 0) { wb_emitter_set_channel(emitter,3); perf_data[0] = (double)robot_id; perf_data[1] = (double)(pkt_count-1); perf_data[2] = 0; wb_emitter_send(emitter,perf_data,3*sizeof(double)); break; // stop node } else { send_data(clock); // send measurement data } } wb_robot_cleanup(); return 0; }
void reset() { int i; robotName = wb_robot_get_name(); currentState = FORWARD; // Make sure to initialize the RNG with different values for each thread srand(time(NULL) + (int)&robotName); char e_puck_name[] = "ps0"; char sensorsName[5]; // Emitter and receiver device tags emitterTag = wb_robot_get_device("emitter"); receiverTag = wb_robot_get_device("receiver"); // Configure communication devices wb_receiver_enable(receiverTag, TIME_STEP); wb_emitter_set_range(emitterTag, COMM_RADIUS); wb_emitter_set_channel(emitterTag, COMMUNICATION_CHANNEL); wb_receiver_set_channel(receiverTag, COMMUNICATION_CHANNEL); sprintf(sensorsName, "%s", e_puck_name); for (i = 0; i < NB_SENSORS; i++) { sensors[i] = wb_robot_get_device(sensorsName); wb_distance_sensor_enable(sensors[i], TIME_STEP); if ((i + 1) >= 10) { sensorsName[2] = '1'; sensorsName[3]++; if ((i + 1) == 10) { sensorsName[3] = '0'; sensorsName[4] = (char) '\0'; } } else { sensorsName[2]++; } } wb_differential_wheels_enable_encoders(TIME_STEP); printf("Robot %s is reset\n", robotName); return; }
/* * 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; }
// // Reset the robot controller and initiate the sensors and emitter/receiver // static int reset() { int i; mode =1; emitter = wb_robot_get_device("emitter"); //no emitter_enable here on purpose! receiver = wb_robot_get_device("receiver"); wb_receiver_enable(receiver, TIME_STEP); char text[5]="led0"; for(i=0;i<NB_LEDS;i++) { led[i]=wb_robot_get_device(text); // get a handler to the sensor text[3]++; // increase the device name to "ps1", "ps2", etc. } text[0]='p'; text[1]='s'; text[3]='\0'; text[2]='0'; ps[0] = wb_robot_get_device(text); // proximity sensors text[2]='7'; ps[1] = wb_robot_get_device(text); // proximity sensors text[2]='6'; ps[2] = wb_robot_get_device(text); // proximity sensors text[2]='5'; ps[3] = wb_robot_get_device(text); text[2]='4'; ps[4] = wb_robot_get_device(text); text[2]='3'; ps[5] = wb_robot_get_device(text); text[2]='2'; ps[6] = wb_robot_get_device(text); text[2]='1'; ps[7] = wb_robot_get_device(text); // Enable proximity and floor sensors for(i=0;i<NB_DIST_SENS;i++) { wb_distance_sensor_enable(ps[i],TIME_STEP); //printf("ps[%d] is active\n",i); } //get handles for all the floor sensors text[0]='f'; text[2]='1'; fs[0]=wb_robot_get_device(text); //note that fs[0] is a 1x1 array that holds the center floor sensor DeviceTag //enable the center fs just to try wb_distance_sensor_enable(fs[0],TIME_STEP); //enable center floor sensor //printf("fs[%d] is active\n",1); //if this sensor is active increase the fitness // open files for writing //pF1=fopen("act_in.txt","w+"); pF2=fopen("decoded_pop.txt","w+"); // pF3=fopen("nn_values.txt","w+"); return 1; }