static void display() { const double FONT_SIZE = 0.08; // display team names and current score char text[64]; sprintf(text, "%s - %d", team_names[TEAM_RED], control_data.teams[TEAM_RED].score); wb_supervisor_set_label(0, text, 0.05, 0.01, FONT_SIZE, 0xff0000, 0.0); // red sprintf(text, "%d - %s", control_data.teams[TEAM_BLUE].score, team_names[TEAM_BLUE]); wb_supervisor_set_label(1, text, 0.99 - 0.025 * strlen(text), 0.01, FONT_SIZE, 0x0000ff, 0.0); // blue // display game state or remaining time if (control_data.state == STATE_PLAYING) sprintf(text,"%02d:%02d",(int)(time/60),(int)time%60); else { static const char *STATE_NAMES[5] = { "INITIAL", "READY", "SET", "PLAYING", "FINISHED" }; sprintf(text, STATE_NAMES[control_data.state]); } wb_supervisor_set_label(2, text, 0.51 - 0.015 * strlen(text), 0.1, FONT_SIZE, 0x000000, 0.0); // black // display instant message if (message_steps > 0) wb_supervisor_set_label(3, message, 0.51 - 0.015 * strlen(message), 0.9, FONT_SIZE, 0x000000, 0.0); // black else { // remove instant message wb_supervisor_set_label(3, "", 1, 0.9, FONT_SIZE, 0x000000, 0.0); message_steps = 0; } }
static void set_scores(int b, int y) { char score[16]; sprintf(score, "%d", b); wb_supervisor_set_label(0, score, 0.92, 0.01, 0.1, 0x0000ff, 0.0); /* blue */ sprintf(score, "%d", y); wb_supervisor_set_label(1, score, 0.05, 0.01, 0.1, 0xffff00, 0.0); /* yellow */ }
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; }
// Find the current performance of the swarm. void findPerformance(double swarm[swarmsize][datasize], double perf[swarmsize], double age[swarmsize], char type, int robots, int neighbors[swarmsize][swarmsize]) { double particles[robots][datasize]; double fit[robots]; int i,j,k; // FOR-loop counters for (i = 0; i < swarmsize; i+=robots) { for (j=0;j<robots && i+j<swarmsize;j++) { sprintf(label2,"Particle: %d\n", i+j); wb_supervisor_set_label(1,label2,0.01,0.05,0.05,0xffffff,0); for (k=0;k<datasize;k++) particles[j][k] = swarm[i+j][k]; } // USER MUST IMPLEMENT FITNESS FUNCTION if (type == EVOLVE_AVG) { // Evalute current fitness fitness(particles,fit,neighbors); // TODO : performance as moving average of previous perf and latest fitness // TODO : pay attention to age[] for (j=0;j<robots && i+j<swarmsize;j++) { } } else if (type == EVOLVE) { fitness(particles,fit,neighbors); for (j=0;j<robots && i+j<swarmsize;j++) perf[i+j] = fit[j]; } else if (type == SELECT) { for (j=0;j<robots && i+j<swarmsize;j++) perf[i+j] = 0.0; for (k=0;k<5;k++) { fitness(particles,fit,neighbors); for (j=0;j<robots && i+j<swarmsize;j++) perf[i+j] += fit[j]; } for (j=0;j<robots && i+j<swarmsize;j++) { perf[i+j] /= 5.0; } } } }
// Find the current performance of the swarm. // Higher performance is better void findPerformance(double swarm[swarmsize][datasize], double perf[swarmsize], double age[swarmsize], char type, int robots, int neighbors[swarmsize][swarmsize]) { double particles[robots][datasize]; double fit[robots]; int i,j,k; // FOR-loop counters for (i = 0; i < swarmsize; i+=robots) { for (j=0;j<robots && i+j<swarmsize;j++) { sprintf(label2,"Particle: %d\n", i+j); wb_supervisor_set_label(1,label2,0.01,0.05,0.05,0xffffff,0); for (k=0;k<datasize;k++) particles[j][k] = swarm[i+j][k]; } // USER MUST IMPLEMENT FITNESS FUNCTION if (type == EVOLVE_AVG) { fitness(particles,fit,neighbors); for (j=0;j<robots && i+j<swarmsize;j++) { perf[i+j] = ((age[i+j]-1.0)*perf[i+j] + fit[j])/age[i+j]; age[i+j]++; } } else if (type == EVOLVE) { fitness(particles,fit,neighbors); for (j=0;j<robots && i+j<swarmsize;j++) perf[i+j] = fit[j]; } else if (type == SELECT) { for (j=0;j<robots && i+j<swarmsize;j++) perf[i+j] = 0.0; for (k=0;k<5;k++) { fitness(particles,fit,neighbors); for (j=0;j<robots && i+j<swarmsize;j++) perf[i+j] += fit[j]; } for (j=0;j<robots && i+j<swarmsize;j++) { perf[i+j] /= 5.0; } } } }
/* n_datasize: number of elements in particle */ double* pso(int n_swarmsize, int n_nb, double lweight, double nbweight, double vmax, double min, double max, int iterations, int n_datasize, int n_robots) { double swarm[n_swarmsize][n_datasize]; // Swarm of particles double perf[n_swarmsize]; // Current local performance of swarm double lbest[n_swarmsize][n_datasize]; // Current best local swarm double lbestperf[n_swarmsize]; // Current best local performance double lbestage[n_swarmsize]; // Life length of best local swarm double nbbest[n_swarmsize][n_datasize]; // Current best neighborhood double nbbestperf[n_swarmsize]; // Current best neighborhood performance double v[n_swarmsize][n_datasize]; // Preference indicator int neighbors[n_swarmsize][n_swarmsize]; // Neighbor matrix int i,j,k; // FOR-loop counters double bestperf; // Performance of evolved solution // Set global variables swarmsize = n_swarmsize; datasize = n_datasize; robots = n_robots; nb = n_nb; printf("NOISY = %d\n",NOISY); sprintf(label, "Iteration: 0"); wb_supervisor_set_label(0,label,0.01,0.01,0.1,0xffffff,0); // Seed the random generator //srand(time(NULL)); srand(0); // Setup neighborhood for (i = 0; i < swarmsize; i++) { for (j = 0; j < swarmsize; j++) { if (mod(i-j+nb,swarmsize) <= 2*nb) neighbors[i][j] = 1; else neighbors[i][j] = 0; } } // Initialize the swarm for (i = 0; i < swarmsize; i++) { for (j = 0; j < datasize; j++) { // Randomly assign initial value in [min,max] swarm[i][j] = (max-min)*rnd()+min; lbest[i][j] = swarm[i][j]; // Best configurations are initially current configurations nbbest[i][j] = swarm[i][j]; v[i][j] = 2.0*vmax*rnd()-vmax; // Random initial velocity } } // Best performances are initially current performances findPerformance(swarm,perf,NULL,EVOLVE,robots,neighbors); for (i = 0; i < swarmsize; i++) { lbestperf[i] = perf[i]; lbestage[i] = 1.0; // One performance so far nbbestperf[i] = perf[i]; } updateNBPerf(lbest,lbestperf,nbbest,nbbestperf,neighbors); // Find best neighborhood performances #if VERBOSE == 1 printf("Swarm initialized\n"); #endif // Run optimization for (k = 0; k < ITS_COEFF*iterations; k++) { #if VERBOSE == 1 printf("Iteration %d\n",k); #endif sprintf(label, "Iteration: %d",k); wb_supervisor_set_label(0,label,0.01,0.01,0.1,0xffffff,0); // Update preferences and generate new particles for (i = 0; i < swarmsize; i++) { for (j = 0; j < datasize; j++) { // Adjust preferences v[i][j] *= 0.6; v[i][j] += lweight*rnd()*(lbest[i][j] - swarm[i][j]) + nbweight*rnd()*(nbbest[i][j] - swarm[i][j]); swarm[i][j] += v[i][j]; } } // RE-EVALUATE PERFORMANCES OF PREVIOUS BESTS #if NOISY == 1 findPerformance(lbest,lbestperf,lbestage,EVOLVE_AVG,robots,neighbors); #endif // Find new performance findPerformance(swarm,perf,NULL,EVOLVE,robots,neighbors); // Update best local performance updateLocalPerf(swarm,perf,lbest,lbestperf,lbestage); // Update best neighborhood performance updateNBPerf(lbest,lbestperf,nbbest,nbbestperf,neighbors); #if VERBOSE == 1 double temp[datasize]; bestperf = bestResult(lbest,lbestperf,temp); printf("%f\n",bestperf); #endif } // Find best result achieved double* best; best = malloc(sizeof(double)*datasize); findPerformance(lbest,lbestperf,NULL,SELECT,robots,neighbors); bestperf = bestResult(lbest,lbestperf,best); #if VERBOSE == 1 printf("Best performance found\n"); printf("Performance: %f\n",bestperf); #endif sprintf(label, "Optimization process over."); wb_supervisor_set_label(0,label,0.01,0.01,0.1,0xffffff,0); sprintf(label2," "); wb_supervisor_set_label(1,label2,0.01,0.05,0.05,0xffffff,0); return best; }
// //main controller of the evolution, this function never returns // static int run() { const double *fit; double finished=0; //double values[4]= {0.0,1.0,0.0,0.0}; //int epoch, reset_position; // as long as individual is being evaluated, print current fitness and return int n = wb_receiver_get_queue_length(receiver); if (n) { fit = (double *)wb_receiver_get_data(receiver); //gets msg[4]={fitness, finished,epoch,reset} fitness[evaluated_inds]=fit[0]; finished=fit[1]; //epoch= (int) fit[2]; //reset_position= (int) fit[3]; //if(reset_position) resetRobotPosition(); //if(1 == epoch || 3 == epoch){ //wb_supervisor_field_set_sf_rotation(pattern_rotation, values); //printf("Flag: %d, world is set.\n", epoch); //}else{ //values[3]=pi; //wb_supervisor_field_set_sf_rotation(pattern_rotation, values); //printf("\n Flag: %d, world is rotated.\n", epoch); //} // print generational info on screen char message[100]; sprintf(message, "Gen: %d Ind: %d Fit: %.2f", generation, evaluated_inds, fit[0]); wb_supervisor_set_label(0, message, 0, 0, 0.1, 0xff0000,0); wb_receiver_next_packet(receiver); } // if still evaluating the same individual, return. if (!finished) return TIME_STEP; //if not finished, !finished ==1. // when evaluation is done, an extra flag is returned in the message if(EVOLVING) { // if whole population has been evaluated if ((evaluated_inds+1) == POP_SIZE ) { // sort population by fitness sortPopulation(); // find and log current and absolute best individual bestfit=sortedfitness[0][0]; bestind=(int)sortedfitness[0][1]; if (bestfit > abs_bestfit) { abs_bestfit=bestfit; abs_bestind=bestind; logBest(); } //printf("best fit: %f\n", bestfit); //write data to files logPopulation(); //rank population, select best individuals and create new generation createNewPopulation(); generation++; if(200==generation) return 0; //printf("\nGENERATION %d\n", generation); evaluated_inds = 0; avgfit = 0.0; bestfit = -1.0; bestind = -1.0; resetRobotPosition(); wb_emitter_send(emitter, (void *)pop_bin[evaluated_inds], GENOME_LENGTH*sizeof(_Bool)); } else { // assign received fitness to individual //printf("fitness: %f\n", fitness[evaluated_inds]); evaluated_inds++; // send next genome to experiment resetRobotPosition(); wb_emitter_send(emitter, (void *)pop_bin[evaluated_inds], GENOME_LENGTH*sizeof(_Bool)); } } return 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; }