int main() {
double buffer[255];
double fit;
double *rbuffer;
int i;
wb_robot_init();
reset();
receiver_enable(rec,32);
differential_wheels_enable_encoders(64);
robot_step(64);
while (1) {
// Wait for data
while (receiver_get_queue_length(rec) == 0) {
robot_step(64);
}
rbuffer = (double *)wb_receiver_get_data(rec);
// Check for pre-programmed avoidance behavior
if (rbuffer[DATASIZE] == -1.0) {
fitfunc(gwaypoints,100);
// Otherwise, run provided controller
} else {
fit = fitfunc(rbuffer,rbuffer[DATASIZE]);
buffer[0] = fit;
wb_emitter_send(emitter,(void *)buffer,sizeof(double));
}
wb_receiver_next_packet(rec);
}
return 0;
}
static void reset(void)
{
int i;
emitter = robot_get_device("emitter");
buffer = (float *) emitter_get_buffer(emitter);
/*
* We try to get a handler to the robots, if this was successful we keep
* a track of some of the fields of the robot and we set its controller.
*/
for (i = 0; i < ROBOTS; i++) {
robot[i] = supervisor_node_get_from_def(robot_name[i]);
/*
* The call to this function is mandatory for the function
* supervisor_node_was_found to be able to work correctly.
*/
robot_step(0);
if (supervisor_node_was_found(robot[i]) == 1) {
supervisor_field_get(robot[i],
SUPERVISOR_FIELD_TRANSLATION_X |
SUPERVISOR_FIELD_TRANSLATION_Z |
SUPERVISOR_FIELD_ROTATION_Y |
SUPERVISOR_FIELD_ROTATION_ANGLE,
&position[i * 4], TIME_STEP);
} else {
robot_console_printf("Error: node %s not found\n", robot_name[i]);
}
}
srand(time(NULL));
return;
}
static void run(void)
{
robot_console_printf("\n\n\n");
int i;
float y[2*NB_CPG];
float dy[2*NB_CPG];
float x[NB_CPG];
float delta = 0.0;
float speed = 0.0;
float drive =0.0;
char speed_char[10];
char drive_char[10];
/*for (i = 0;i<7;i++) {
old_pos[i] = current_position[i];
}*/
/*float initial_conditions[2*NB_CPG]={0.0, 0.0, 0.0, 0.0,
0.1, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.1,
0.0, 0.0, 0.0, 0.0,
0.2, 0.1};*/
//base values
//FILE* fileID = fopen("recherche_syst.m", "w");
//fprintf(fileID, "Z = [ ");
float v;
float R[NB_CPG];
for(i=0; i<2*NB_CPG; ++i)
{
y[i] = 0.2; //initial_conditions[i]; // (float)(9-(i/2))/10.0;
dy[i] = 0.0;
}
// run TIME miliseconds
for(i = 0 ; i < TIME ; i += TIME_STEP)
{
drive=getDrive(drive);
v = drive;
set_ampl(v, R);
fcn(y, dy, v, R, current_movement);
euler(y,dy,TIME_STEP);
next_step(x,y);
//fprintf(fileID," %f %f %f %f %f %f ;\n", y[2]-y[0], y[4]-y[0], y[6]-y[2], y[6]-y[4], y[6]-y[0], y[2]-y[4]);
// Calculate speed
if (!(i%(TIME_STEP*50))) {
//Every 50 TIME_STEP to avoid oscillations (= 800ms)
delta = pow(pow((current_position[0]- old_pos[0]),2) + pow((current_position[2]-old_pos[2]),2), 0.5);
old_pos[0] = current_position[0];
old_pos[2] = current_position[2];
speed = 1000*delta/((float)50*TIME_STEP);
}
//robot_console_printf("Speed : %d, ,%f, %f, %f, %f\n", i, delta, speed, old_pos[0], current_position[0]);
elapsed_time += (float) TIME_STEP / 1000;
/*if (elapsed_time > 5.0 && abs(delta) < 0.000001) {//abs(current_position[6]) > 2) {
break;
}*/
// Print informations
sprintf(drive_char, "%.2f", drive);
sprintf(speed_char, "%.2f", speed);
supervisor_set_label(3, drive_char, 0.16, 0.01, 0.04, 0x0000ff); /* blue movement */
supervisor_set_label(4, getCurrentMovementString(), 0.16, 0.045, 0.04, 0x0000ff); /* blue movement */
supervisor_set_label(5, speed_char, 0.16, 0.08, 0.04, 0xff0000); /* red speed */
// actuate front legs
servo_set_position(servos[0], x[0]);
servo_set_position(servos[2], x[2]);
//actuate front kmees
servo_set_position(servos[5], x[4]);
servo_set_position(servos[7], x[6]);
// actuate back legs
servo_set_position(servos[1], x[1]);
servo_set_position(servos[3], x[3]);
//actuate back knees
servo_set_position(servos[6], x[5]);
servo_set_position(servos[8], x[7]);
//actuate spine
servo_set_position(servos[4], x[8]);
robot_step(TIME_STEP);
}
// Reset
servo_set_position(servos[0], 0);
servo_set_position(servos[1], 0);
servo_set_position(servos[2], 0);
servo_set_position(servos[3], 0);
servo_set_position(servos[4], 0);
servo_set_position(servos[5], 0);
servo_set_position(servos[6], 0);
servo_set_position(servos[7], 0);
servo_set_position(servos[8], 0);
const float FLOAT_POS[7] = { 0, 1000, 0, 0, 1, 0, 0 };
supervisor_field_set(ghostdog, SUPERVISOR_FIELD_TRANSLATION_AND_ROTATION,(void*)FLOAT_POS);
robot_step(10*TIME_STEP);
const float INITIAL_POSITION_AND_ROTATION[7] = { 0, 0.3, 0, 0, 1, 0, 0 };
supervisor_field_set(ghostdog, SUPERVISOR_FIELD_TRANSLATION_AND_ROTATION,(void*)INITIAL_POSITION_AND_ROTATION);
elapsed_time = 0.0;
//fclose(fileID);
}
// Find the fitness for obstacle avoidance of the passed controller
double fitfunc(double waypoints[DATASIZE],int its) {
double fitness = 0.0; // Fitness of waypoint set
int j,i;
int left_encoder, right_encoder, old_left_encoder = 0, old_right_encoder = 0;
double theta = 0.0, old_theta = 0.0;
double x = 0.0, y = 0.0;
int k = 0;
int ds_l, ds_r, lspeed, rspeed;
double fit_ds, fit_speed, fit_totalds, fitness_distance, fitness_stuck, fitness_goal, fitness_waypoints;
fit_speed = 0.0;
fit_ds = 0.0;
fit_totalds = 0.0;
int count_stuck = 0;
int count_waypoints = 0;
int count_waypoints_actual = 0;
fitness_goal = 0.333;
double waypoints_reached[DATASIZE];
for (i=0; i<DATASIZE; i++) {
waypoints_reached[i] = 0.0;
}
wb_differential_wheels_set_encoders(0,0);
// Evaluate fitness repeatedly
for (j=0; j<its; j++) {
// if close enough to current waypoint select next waypoint
if ((x - waypoints[k])*(x - waypoints[k])+(y - waypoints[k+1])*(y - waypoints[k+1]) < 0.0001) {
//printf("WAYPOINT REACHED (%.2f,%.2f) YESSS \n",waypoints[k], waypoints[k+1]);
count_waypoints += 1;
count_waypoints_actual += 1;
waypoints_reached[k] = waypoints[k];
waypoints_reached[k+1] = waypoints[k+1];
k += 2;
loc_state = ALIGN;
if (k == DATASIZE) {
k = 0;
}
}
// compute wheel speeds
compute_wheel_speeds(x,y,theta,waypoints[k],waypoints[k+1],&lspeed,&rspeed);
wb_differential_wheels_set_speed(lspeed,rspeed);
robot_step(128); // run one step
// compute current position
left_encoder = wb_differential_wheels_get_left_encoder();
right_encoder = wb_differential_wheels_get_right_encoder();
ds_l = left_encoder - old_left_encoder;
ds_r = right_encoder - old_right_encoder;
odometry(ds_l, ds_r, x, y, old_theta, &x, &y, &theta, &fit_ds);
old_left_encoder = left_encoder;
old_right_encoder = right_encoder;
old_theta = theta;
fit_speed += (fabs(lspeed) + fabs(rspeed))/(2.0*MAX_SPEED);
fit_totalds += fit_ds;
if(fit_ds < 0.0001) {
count_stuck += 1;
}
if (y > 1.2) { // when this coordinate is reached the robot is considered escaped and the evaluation stops
printf("\nROBOT ESCAPED!! :)\n");
for (i=0; i<DATASIZE; i++) {
printf("%f ",waypoints_reached[i]);
}
printf("\n");
count_waypoints = 5; //this acts as a bonus for reaching the goal
fitness_goal = 1.0;
break;
}
}
fit_speed /= its;
fitness_distance = 1/(1+exp(fit_totalds-4));
fitness_stuck = (1 - (double)count_stuck/its);
fitness_goal = fitness_goal;
fitness_waypoints = pow((((double)count_waypoints/10)+0.5),2); //1.2 are assumed to be the length of sides of the world
fitness = pow(fitness_distance,0.75)*pow(fitness_stuck,1.5)*fitness_goal*fitness_waypoints;
/*
if (fitness > fitness_best) {
fitness_best = fitness;
printf("\nFITNESS: %f\nTotal Distance: %.2f\nStuck Ratio: %.2f\n\n", fitness, fit_totalds, fitness_stuck);
}
*/
printf("FITNESS: %f waypoints: %d ditance: %.2f stuck: %.2f\n", fitness, count_waypoints_actual, fit_totalds, fitness_stuck);
return fitness;
}
