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;
}
