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;
}
// the main function
int main(){
int msl, msr; // Wheel speeds
double *rbuffer;
double buffer[255];
int j;
for(;;) {
reset(); // Resetting the robot
while (wb_receiver_get_queue_length(receiver0) == 0)
wb_robot_step(64);
rbuffer = (double *)wb_receiver_get_data(receiver0);
COEF_KP = rbuffer[0];
COEF_B = rbuffer[1];
COEF_R = rbuffer[2];
COEF_S = rbuffer[3];
COEF_T = rbuffer[4];
msl = 0; msr = 0;
// Forever
for(j = 0; j < NB_STEPS; ++j) {
/* Send and get information */
send_ping(); // sending a ping to other robot, so they can measure their distance to this robot
process_received_ping_messages();
// Compute self position
prev_my_position[0] = my_position[0];
prev_my_position[1] = my_position[1];
update_self_motion(msl,msr);
speed[robot_id][0] = (1/DELTA_T)*(my_position[0]-prev_my_position[0]);
speed[robot_id][1] = (1/DELTA_T)*(my_position[1]-prev_my_position[1]);
// Flocking behavior of the paper with the wheels speed
flocking_behavior(&msl, &msr);
// Set speed
wb_differential_wheels_set_speed(msl,msr);
// Continue one step
wb_robot_step(TIME_STEP);
}
wb_emitter_send(emitter0,(void *)buffer,sizeof(double));
wb_receiver_next_packet(receiver0);
}
}
int main()
{
/* necessary to initialize webots stuff */
wb_robot_init();
reset();
int result=0;
/* main loop
* Perform simulation steps of TIME_STEP milliseconds
* and leave the loop when the simulation is over
*/
while (wb_robot_step(TIME_STEP) != -1) {
result=run();
if (result!=TIME_STEP) break;
};
closeFiles();
// quit the sim
wb_supervisor_simulation_quit(EXIT_SUCCESS);
/* This is necessary to cleanup webots resources */
wb_robot_cleanup();
return 0;
}
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;
}
int main() {
double w_left, w_right;
wb_robot_init();
WbDeviceTag human02_wheel;
human02_wheel = wb_robot_get_device("human02_plate");
int time_step = wb_robot_get_basic_time_step();
wb_differential_wheels_enable_encoders(time_step);
for (;;)
{
w_left = 10.;
w_right = 10.;
wb_differential_wheels_set_speed(w_left, w_right);
//printf(" left_encoder=%7.5f\n",wb_differential_wheels_get_left_encoder());
wb_robot_step(32);
}
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()
{
/* necessary to initialize webots stuff */
initialize();
//Initially, the puck just heads straight.
headStraight();
/* main loop
* Perform simulation steps of TIME_STEP milliseconds
* and leave the loop when the simulation is over
*/
while (wb_robot_step(TIME_STEP) != -1)
{
//As long as there is time, keep taking lightTrackerSteps;
lightTrackerStep();
};
/* Enter your cleanup code here */
/* This is necessary to cleanup webots resources */
wb_robot_cleanup();
return 0;
}
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;
}
int main() {
srand ( time(NULL) );
wb_robot_init();
wb_robot_step(TIME_STEP);
wb_robot_step(TIME_STEP);
/* main loop */
while (wb_robot_step(TIME_STEP) != -1) {
wb_differential_wheels_set_speed(1000,1000);
}
wb_robot_cleanup();
return 0;
}
int main(int argc, char *argv[]) {
/* initialize Webots */
wb_robot_init();
reset();
/* perform a simulation step */
wb_robot_step(TIME_STEP);
/* main loop */
while (wb_robot_step(TIME_STEP) != -1) {
run();
}
return 0;
}
int main(int argc, char *args[]) {
char buffer[255]; // Buffer for sending data
int i; // Index
if (argc == 4) { // Get parameters
offset = atoi(args[1]);
migrx = atof(args[2]);
migrz = atof(args[3]);
//migration goal point comes from the controller arguments. It is defined in the world-file, under "controllerArgs" of the supervisor.
printf("Migratory instinct : (%f, %f)\n", migrx, migrz);
} else {
printf("Missing argument\n");
return 1;
}
orient_migr = -atan2f(migrx,migrz);
if (orient_migr<0) {
orient_migr+=2*M_PI; // Keep value within 0, 2pi
}
reset();
send_init_poses();
// Compute reference fitness values
float fit_cluster; // Performance metric for aggregation
float fit_orient; // Performance metric for orientation
for(;;) {
wb_robot_step(TIME_STEP);
if (t % 10 == 0) {
for (i=0;i<FLOCK_SIZE;i++) {
// Get data
loc[i][0] = wb_supervisor_field_get_sf_vec3f(robs_trans[i])[0]; // X
loc[i][1] = wb_supervisor_field_get_sf_vec3f(robs_trans[i])[2]; // Z
loc[i][2] = wb_supervisor_field_get_sf_rotation(robs_rotation[i])[3]; // THETA
// Sending positions to the robots, comment the following two lines if you don't want the supervisor sending it
sprintf(buffer,"%1d#%f#%f#%f##%f#%f",i+offset,loc[i][0],loc[i][1],loc[i][2], migrx, migrz);
wb_emitter_send(emitter,buffer,strlen(buffer));
}
//Compute and normalize fitness values
compute_fitness(&fit_cluster, &fit_orient);
fit_cluster = fit_cluster_ref/fit_cluster;
fit_orient = 1-fit_orient/M_PI;
printf("time:%d, Topology Performance: %f\n", t, fit_cluster);
}
t += TIME_STEP;
}
}
static void terminate() {
if (match_type != DEMO) {
// terminate movie recording and quit
wb_supervisor_stop_movie();
wb_robot_step(0);
wb_supervisor_simulation_quit();
}
while (1) step(); // wait forever
}
int main() {
const char *SERVO_NAMES[NUM_SERVOS] = {
"servo_r0",
"servo_r1",
"servo_r2",
"servo_l0",
"servo_l1",
"servo_l2"};
WbDeviceTag servos[NUM_SERVOS];
int elapsed = 0;
int state, i;
long double pos = 0,lastPos = 0; // int 害死了 0-6.28
int flag = 0;
const int dir[] = {1,1,1,1,1,1};//{1,1,1,-1,-1,-1}
// 增加在地面上的时间，就会更加稳定
const double rate = 1.0/6;// 1/4
wb_robot_init();
for (i = 0; i < NUM_SERVOS; i++) {
servos[i] = wb_robot_get_device(SERVO_NAMES[i]);
if (!servos[i]) {
printf("could not find servo: %s\n",SERVO_NAMES[i]);
}
}
pos = M_PI * rate ;
flag = 0;
while(wb_robot_step(TIME_STEP)!=-1) {
elapsed++;
state = (elapsed / 25 + 1) % NUM_STATES;
lastPos = pos;
if(flag) pos += 2*M_PI * rate;
else pos += 2*M_PI * (1-rate);
flag = !flag;
for (i = 0; i < 6; i+=2 ){
wb_servo_set_position(servos[i], dir[i]*pos);
}
for (i = 1; i < 6; i+=2 ) {
wb_servo_set_position(servos[i], dir[i]*lastPos);
}
}
wb_robot_cleanup();
return 0;
}
int main() {
wb_robot_init();
reset();
while(wb_robot_step(TIME_STEP) != -1) {
run();
}
wb_robot_cleanup();
return 0;
}
// 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;
}
int main() {
int duration = TIME_STEP;
wb_robot_init(); // controller initialization
reset();
while (wb_robot_step(duration) != -1) {
duration = run(duration);
}
wb_robot_cleanup();
return 0;
}
int main(int argc, char *argv[]) {
/* initialize Webots */
wb_robot_init();
reset();
/* main loop */
for (;;) {
getSensorValues(distances);
run();
/* perform a simulation step */
wb_robot_step(TIME_STEP);
}
return 0;
}
int main(int argc, char **argv)
{
//robot_live(reset);
// robot_run(run);
/* necessary to initialize webots stuff */
wb_robot_init();
//TIME_STEP = (int)wb_robot_get_basic_time_step();
int n=reset();
printf("did reset: %d\n",n);
/*
* 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");
*/
/* main loop
* Perform simulation steps of TIME_STEP milliseconds
* and leave the loop when the simulation is over
*/
while (wb_robot_step(TIME_STEP) != -1) {
/*
* Read the sensors :
* Enter here functions to read sensor data, like:
* double val = wb_distance_sensor_get_value(my_sensor);
*/
run(TIME_STEP);
/* Process sensor data here */
/*
* Enter here functions to send actuator commands, like:
* wb_differential_wheels_set_speed(100.0,100.0);
*/
};
/* Enter your cleanup code here */
/* This is necessary to cleanup webots resources */
wb_robot_cleanup();
return 0;
}
void send_init_poses(void)
{
char buffer[255]; // Buffer for sending data
int i;
for (i=0;i<FLOCK_SIZE;i++) {
// Get data
loc[i][0] = wb_supervisor_field_get_sf_vec3f(robs_trans[i])[0]; // X
loc[i][1] = wb_supervisor_field_get_sf_vec3f(robs_trans[i])[2]; // Z
loc[i][2] = wb_supervisor_field_get_sf_rotation(robs_rotation[i])[3]; // THETA
printf("Supervisor %d %d %d/n",loc[i][0],loc[i][0],loc[i][0]);
// Send it out
sprintf(buffer,"%1d#%f#%f#%f##%f#%f",i+offset,loc[i][0],loc[i][1],loc[i][2], migrx, migrz);
wb_emitter_send(emitter,buffer,strlen(buffer));
// Run one step
wb_robot_step(TIME_STEP);
}
}
int main(int argc, char **argv)
{
wb_robot_init();
int time_step = wb_robot_get_basic_time_step();
WbDeviceTag gps = wb_robot_get_device("GPS");
wb_gps_enable(gps, time_step);
WbDeviceTag emitter = wb_robot_get_device("emitter");
wb_emitter_set_channel(emitter,13);
double* gps_value;
char message[32];
while (wb_robot_step(time_step) != -1){
gps_value = wb_gps_get_values(gps);
//Something's wrong with deserialization
//So we make a fixed width string here
sprintf(message,"{%0.3f,%0.3f}",gps_value[0]/2+5.0,-gps_value[2]/2+5.0);
wb_emitter_send(emitter,message,strlen(message)+1);
// printf("%s\n",message);
}
wb_robot_cleanup();
return 0;
}
int main(int argc, char **argv)
{
wb_robot_init();
int i;
WbDeviceTag servos[NSERVOS];
WbDeviceTag head_led;
for (i=0; i<NSERVOS; i++)
servos[i] = wb_robot_get_device(servo_names[i]);
head_led = wb_robot_get_device("HeadLed");
wb_led_set(head_led, 0x40C040);
do {
double t = wb_robot_get_time();
for (i=0; i<6; i++)
wb_servo_set_position(servos[i], amplitudes[i]*sin(frequency*t) + offsets[i]);
} while (wb_robot_step(TIME_STEP) != -1);
wb_robot_cleanup();
return 0;
}
// this is what is done at every time step independently of the game state
static void step() {
// copy pointer to ball position values
if (ball_translation)
ball_pos = wb_supervisor_field_get_sf_vec3f(ball_translation);
int i;
for (i = 0; i < NUM_ROBOTS; i++) {
// copy pointer to robot position values
if (robot_translation[i])
robot_pos[i] = wb_supervisor_field_get_sf_vec3f(robot_translation[i]);
if (robot_rotation[i])
robot_rot[i] = wb_supervisor_field_get_sf_rotation(robot_rotation[i]);
}
if (message_steps)
message_steps--;
// yield control to simulator
wb_robot_step(TIME_STEP);
// every 480 milliseconds
if (step_count++ % 12 == 0)
sendGameControlData();
//checkFalling();
// did I receive a message ?
//read_incoming_messages();
// read key pressed
check_keyboard();
}
int main(int argc, char *args[]) {
int i; // Index
reset();
// initialize fitness values
float fit_O;
float fit_C;
float fit_V;
float fit_P;
float fit_S;
float perf_sum = 0.0f;
int nb_measur = 0; //number of measurment of instant perf
for(;;) {
wb_robot_step(TIME_STEP);
if (t % 100 == 0) {
for (i=0;i<FLOCK_SIZE;i++) {
// initialize old position
loc_old[i][0] = loc[i][0];
loc_old[i][1] = loc[i][1];
loc_old[i][2] = loc[i][2];
// initialize current position
loc[i][0] = wb_supervisor_field_get_sf_vec3f(robs_trans[i])[0];
loc[i][1] = wb_supervisor_field_get_sf_vec3f(robs_trans[i])[2];
loc[i][2] = wb_supervisor_field_get_sf_rotation(robs_rotation[i])[3];
}
// compute the orientation metric
compute_fitness_O(& fit_O);
// compute the cohesion metric
compute_fitness_C(& fit_C);
// compute the velocity metric
compute_fitness_V(& fit_V);
// compute entropy metric
compute_fitness_S(& fit_S);
// compute total metric value
fit_P = instant_perf();
// Display fitness
printf("time : %d, orientation , cohesion , velocity , entropy, instant perf : %.4lf, %.4lf, %.4lf, %.4lf, %.4lf\n", t, fit_O, fit_C, fit_V, fit_S, fit_P);
//avoid wrong values
if(fit_P > 0){
perf_sum += fit_P;
nb_measur++;
}
}
if (t % 1000 == 0){
printf("time : %d, overall perf : %.4lf\n", t, perf_sum/nb_measur);
}
t += TIME_STEP;
}
}
/*
* Main function.
*/
int main(int argc, char *args[]) {
float fitness;
// The type of formation is decided by the user through the world
char* formation = DEFAULT_FORMATION;
if(argc == 2) {
formation_type = atoi(args[1]);
}
if(formation_type == 0)
formation = "line";
else if(formation_type == 1)
formation = "column";
else if(formation_type == 2)
formation = "wedge";
else if(formation_type == 3)
formation = "diamond";
printf("Chosen formation: %s.\n", formation);
////////////////////////////////////////////////////////////////////////////////////////////////////
// PSO //
////////////////////////////////////////////////////////////////////////////////////////////////////
// The paramethers that we don't optimise are commented.
// To add them to the simulation you should uncomment it here, in pso_ocba and change DIMENSIONALITY
// each motorschema's weight
parameter_ranges[0][0] = 0;
parameter_ranges[0][1] = 10;
parameter_ranges[1][0] = 0;
parameter_ranges[1][1] = 10;
parameter_ranges[2][0] = 0;
parameter_ranges[2][1] = 10;
parameter_ranges[3][0] = 0;
parameter_ranges[3][1] = 10; /*
parameter_ranges[4][0] = 0;
parameter_ranges[4][1] = 4;*/
/*
// thresholds
parameter_ranges[ 5][0] = 0.001; // obstacle avoidance min
parameter_ranges[ 5][1] = 0.5;
parameter_ranges[ 6][0] = 0.001; // obstacle avoidance min + range
parameter_ranges[ 6][1] = 1;
parameter_ranges[ 7][0] = 0.001; // move_to_goal min
parameter_ranges[ 7][1] = 0.5;
parameter_ranges[ 8][0] = 0.001; // move_to_goal min + range
parameter_ranges[ 8][1] = 1;
parameter_ranges[ 9][0] = 0.001; // avoid_robot min
parameter_ranges[ 9][1] = 0.2;
parameter_ranges[10][0] = 0.001; // avoid_robot min + range
parameter_ranges[10][1] = 1;
parameter_ranges[11][0] = 0.001; // keep_formation min
parameter_ranges[11][1] = 0.3;
parameter_ranges[12][0] = 0.001; // keep_formation min + range
parameter_ranges[12][1] = 1;
// noise
parameter_ranges[13][0] = 1; // noise_gen frequency
parameter_ranges[13][1] = 20;
parameter_ranges[14][0] = 0; // fade or not
parameter_ranges[14][1] = 1;
*/
float optimal_params[DIMENSIONALITY];
initialize();
reset();
pso_ocba(optimal_params);
// each motorschema's weight
w_goal = optimal_params[0];
w_keep_formation = optimal_params[1];
w_avoid_robot = optimal_params[2];
w_avoid_obstacles = optimal_params[3];/*
w_noise = optimal_params[4];
// thresholds
avoid_obst_min_threshold = optimal_params[5];
avoid_obst_max_threshold = optimal_params[5] + optimal_params[6];
move_to_goal_min_threshold = optimal_params[7];
move_to_goal_max_threshold = optimal_params[7] + optimal_params[8];
avoid_robot_min_threshold = optimal_params[9];
avoid_robot_max_threshold = optimal_params[9] + optimal_params[10];
keep_formation_min_threshold = optimal_params[11];
keep_formation_max_threshold = optimal_params[11] + optimal_params[12];
// noise parameters
noise_gen_frequency = optimal_params[13];
fading = round(optimal_params[14]); // = 0 or 1
*/
printf("\n\n\nThe optimal parameters are: \n");
printf("___________ w_goal...................... = %f\n", w_goal);
printf("___________ w_keep_formation............ = %f\n", w_keep_formation);
printf("___________ w_avoid_robo................ = %f\n", w_avoid_robot);
printf("___________ w_avoid_obstacles........... = %f\n", w_avoid_obstacles);
printf("___________ w_noise..................... = %f\n", w_noise);
printf("___________ noise_gen_frequency......... = %d\n", noise_gen_frequency);
printf("___________ fading...................... = %d\n", fading);
printf("___________ avoid_obst_min_threshold.... = %f\n", avoid_obst_min_threshold);
printf("___________ avoid_obst_max_threshold.... = %f\n", avoid_obst_max_threshold);
printf("___________ move_to_goal_min_threshold.. = %f\n", move_to_goal_min_threshold);
printf("___________ move_to_goal_max_threshold.. = %f\n", move_to_goal_max_threshold);
printf("___________ avoid_robot_min_threshold... = %f\n", avoid_robot_min_threshold);
printf("___________ avoid_robot_max_threshold... = %f\n", avoid_robot_max_threshold);
printf("___________ keep_formation_min_threshold = %f\n", keep_formation_min_threshold);
printf("___________ keep_formation_max_threshold = %f\n", keep_formation_max_threshold);
printf("\n\n");
////////////////////////////////////////////////////////////////////////////////////////////////////
// REAL RUN //
////////////////////////////////////////////////////////////////////////////////////////////////////
// Real simulation with optimized parameters:
initialize();
// reset and communication part
printf("Beginning of the real simulation\n");
reset_to_initial_values();
printf("Supervisor reset.\n");
send_real_run_init_poses();
printf("Init poses sent.\n Chosen formation: %s.\n", formation);
// sending weights
send_weights();
printf("Weights sent\n");
bool is_goal_reached=false;
// infinite loop
for(;;) {
wb_robot_step(TIME_STEP);
// every 10 TIME_STEP (640ms)
if (simulation_duration % 10 == 0) {
send_current_poses();
update_fitness();
}
simulation_duration += TIME_STEP;
if (simulation_has_ended()) {
is_goal_reached=true;
printf("\n\n\n\n______________________________________GOAL REACHED!______________________________________\n\n");
// stop the robots
w_goal = 0;
w_keep_formation = 0;
w_avoid_robot = 0;
w_avoid_obstacles = 0;
w_noise = 0;
send_weights();
break;
}
}
if (is_goal_reached){
fitness = compute_fitness(FORMATION_SIZE,loc);
} else {
fitness = compute_fitness(FORMATION_SIZE,loc);
}
printf("fitness = %f\n",fitness);
while (1) wb_robot_step(TIME_STEP); //wait forever
return 0;
}
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;
}
int main(int argc, const char *argv[]) {
WbDeviceTag speaker, microphone;
WbDeviceTag sensors[NUM_SENSORS];
int role = UNKNOWN;
int prev_audible = -1;
int i, j; // for loops
// initialize webots
wb_robot_init();
// determine role
if (argc >= 2) {
if (! strcmp(argv[1], "speaker"))
role = SPEAKER;
else if (! strcmp(argv[1], "microphone"))
role = MICROPHONE;
}
// use speaker or microphone according to specified role
if (role == SPEAKER)
speaker = wb_robot_get_device("speaker");
else if (role == MICROPHONE) {
// we only use "mic0" in this demo
microphone = wb_robot_get_device("mic0");
wb_microphone_enable(microphone, TIME_STEP / 2);
}
// find distance sensors
for (i = 0; i < NUM_SENSORS; i++) {
char sensor_name[64];
sprintf(sensor_name, "ps%d", i);
sensors[i] = wb_robot_get_device(sensor_name);
wb_distance_sensor_enable(sensors[i], TIME_STEP);
}
// main loop
for (;;) {
if (role == SPEAKER) {
wb_speaker_emit_sample(speaker, SAMPLE, sizeof(SAMPLE));
}
else if (role == MICROPHONE) {
const short *rec_buffer = (const short *)wb_microphone_get_sample_data(microphone);
int numSamples = wb_microphone_get_sample_size(microphone) / sizeof(SAMPLE[0]);
if (rec_buffer) {
int audible = 0;
for (i = 0; i < numSamples; i++) {
// warning this demo assumes no noise and therefore depends on the sound plugin configuration !
if (rec_buffer[i] != 0)
audible = 1;
}
if (audible != prev_audible) {
printf(audible ? "I hear you now !\n" : "I can't hear you !\n");
prev_audible = audible;
}
}
else
printf("received: nothing this time ...\n");
}
// read distance sensor values
double sensor_values[NUM_SENSORS];
for (i = 0; i < NUM_SENSORS; i++)
sensor_values[i] = wb_distance_sensor_get_value(sensors[i]);
// compute braitenberg collision avoidance
double speed[2];
for (i = 0; i < 2; i++) {
speed[i] = 0.0;
for (j = 0; j < NUM_SENSORS; j++)
speed[i] += EPUCK_MATRIX[j][i] * (1 - (sensor_values[j] / RANGE));
}
// set the motors speed
wb_differential_wheels_set_speed(speed[0], speed[1]);
// simulation 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;
}
int main(){
reset();
int msl,msr; // motor speed left and right
double distances[NB_SENSORS]; // array keeping the distance sensor readings
int elapsed_time; // elapsed time during one time_step
int obstacle_near; // flag which becomes true if an obstacle is nearby
int hit_flag; // flag which becomes true as soon as the robot hits an obstacle
int last_ts_was_hit = 0; // flag memorizing whether the robot was already in a collision during the last
// time step
int time_steps; // counter for simulated time steps
int hit_counter; // counter for time steps involving collisions
int collision_duration; // duration of a single collision (in time steps unit)
double average_collision_duration;// average duration of a single collision
const double* gpsvalues; // matrix containing gps readings
double actx,acty; // current position
double oldx,oldy; // previous position
double speed; // average robot speed in free space
int collision_started = 0;
// iteration variables
int i;
int sensor_nb;
FILE* f; // file handle
char fname[]="/tmp/collision_pos.txt"; // file name to store the positions of the collisions
if(fmod(TIME_STEP,50)!=0){
printf("ERROR: The TIME_STEP needs to be a multiple of 50ms\n");
return(0);
}
for(i=0;i<NB_SENSORS;i++) {
wb_distance_sensor_enable(ps[i],50); // this function wb_*_enable() allows to specify an update interval for the sensor readings in milliseconds
}
wb_gps_enable(gps,50);
wb_robot_step(50);
gpsvalues = wb_gps_get_values(gps); // returns a pointer to three double values. The pointer is the address of an array allocated by the function internally.
wb_robot_step(50);
oldx=gpsvalues[0];
oldy=gpsvalues[2];
time_steps=0;
hit_counter=0;
hit_flag=0;
average_collision_duration=0;
speed=0;
int ts_2=0, ts_3=0, ts_4=0, ts_5=0;
for(;;){
elapsed_time=0; // reset elapsed time and hit flag
ts_5 = ts_4;
ts_4 = ts_3;
ts_3 = ts_2;
ts_2 = last_ts_was_hit;
last_ts_was_hit=hit_flag; // copying the status of the last time step
if(!last_ts_was_hit) {
collision_duration=1; // if not actually colliding, always reset counter
}
hit_flag=0; // every time step
while(elapsed_time<TIME_STEP){
msl=0; msr=0;
for(sensor_nb=0;sensor_nb<NB_SENSORS;sensor_nb++){ // read sensor values and calculate motor speeds
distances[sensor_nb] = wb_distance_sensor_get_value(ps[sensor_nb]);
//printf("t=%d, distance[%d] = %f\n",time_steps,sensor_nb,distances[sensor_nb]);
msr += distances[sensor_nb] * Interconn[sensor_nb];
msl += distances[sensor_nb] * Interconn[sensor_nb + NB_SENSORS];
}
msl /= 350; msr /= 350; // Normalizing speeds
actx=gpsvalues[0];
acty=gpsvalues[2];
//printf("x = %f, y = %f\n",actx, acty);
obstacle_near=(abs(msl)>10 || abs(msr)>10);
if(obstacle_near){ // if motor speed changes (if obstacle is hit)
//printf("obstacle_near=%d msr = %d msl = %d \n", obstacle_near, msr, msl);
hit_flag=1;
f=fopen(fname,"a");
fprintf(f,"%f %f\n",
actx,
acty);
fclose(f);
} else { // measure speed only when no obstacles near
double dist = sqrt((actx-oldx)*(actx-oldx)+(acty-oldy)*(acty-oldy)); // distance traveled in 50ms
if(dist<0.01) // don't consider fast speed due to wrap around
speed=speed*0.999+0.001*dist*1000.0/50.0;
}
oldx=actx;
oldy=acty;
msl += (BIAS_SPEED);
msr += (BIAS_SPEED);
wb_differential_wheels_set_speed(3*msl,3*msr);
gpsvalues = wb_gps_get_values(gps);
wb_robot_step(50); // Executing the simulation for 50ms
elapsed_time+=50;
} // end time step
time_steps++;
if(!(last_ts_was_hit || ts_2 || ts_3 || ts_4 || ts_5) && hit_flag){// increase hit counter only for new collisions
collision_started = 1;
hit_counter++;
//printf("hit_counter=%d msr = %d msl = %d \n",hit_counter, msr, msl);
}
else if(collision_started && hit_flag){
collision_duration++; // otherwise measure duration of a collision
//printf("collision_duration=%d msr = %d msl = %d \n", collision_duration, msr, msl);
}
if(!hit_flag && collision_started){ // at the end of a collision, we calculate the running average
if(collision_duration>4){
collision_started = 0;
printf("end of collision, duration=%d msr = %d msl = %d \n", collision_duration, msr, msl);
collision_duration = collision_duration + 4;
average_collision_duration= (average_collision_duration*(hit_counter-1)+collision_duration)/hit_counter;
f=fopen("/tmp/collision_duration_webots.txt","a");
fprintf(f,"%d \n",collision_duration);
fclose(f);}
else{
//printf("collision too short = %d, thus removed \n", collision_duration);
collision_started = 0;
hit_counter--;
}
}
if(fmod(time_steps,20*60*1) == 0){ // every 1 minutes
printf("Hits: %d Time [time steps]): %d p: %f Avg. Ta [time steps]: %0.2f v (m/s):%0.3f\n",
hit_counter, time_steps,
(double) hit_counter/(time_steps-hit_counter*ROUND(average_collision_duration)),
average_collision_duration,
speed);
f=fopen("/tmp/collision_probability.txt","a");
fprintf(f,"%f\n", (double) hit_counter/(time_steps-hit_counter*ROUND(average_collision_duration)));
fclose(f);
}
}
}
// the main function
int main(){
int msl, msr, bmsl, bmsr; // Wheel speeds
int sum_sensors; // Braitenberg parameters
int i, j; // Loop counter
int max_sens; // Store highest sensor value
double *rbuffer;
double buffer[255];
int distances[NB_SENSORS]; // Array for the distance sensor readings
for(;;) {
reset(); // Resetting the robot
while (wb_receiver_get_queue_length(receiver0) == 0) {
wb_robot_step(64);
}
rbuffer = (double *)wb_receiver_get_data(receiver0);
for(i=0; i<FLOCK_SIZE; i++) {
timestamp[i] = 0;
}
msl = 0; msr = 0;
max_sens = 0;
// Forever
for(j = 0; j < NB_STEPS; ++j){
bmsl = 0; bmsr = 0;
sum_sensors = 0;
max_sens = 0;
/* Braitenberg */
for(i=0;i<NB_SENSORS;i++) {
distances[i]=wb_distance_sensor_get_value(ds[i]); //Read sensor values
sum_sensors += distances[i]; // Add up sensor values
max_sens = max_sens>distances[i]?max_sens:distances[i]; // Check if new highest sensor value
// Weighted sum of distance sensor values for Braitenburg vehicle
bmsr += rbuffer[i] * distances[i];
bmsl += rbuffer[i+NB_SENSORS] * distances[i];
}
// Adapt Braitenberg values (empirical tests)
bmsl/=MIN_SENS; bmsr/=MIN_SENS;
bmsl+=66; bmsr+=72;
/* Send and get information */
timestamp[robot_id]++;
send_ping_robot(robot_id);
process_received_ping_messages();
//printf("ROBOT %d: wheels %d, %d\n", robot_id, bmsl, bmsr);
// Compute self position
prev_my_position[0] = my_position[0];
prev_my_position[1] = my_position[1];
update_self_motion(msl,msr);
speed[robot_id][0] = (1/DELTA_T)*(my_position[0]-prev_my_position[0]);
speed[robot_id][1] = (1/DELTA_T)*(my_position[1]-prev_my_position[1]);
// Reynold's rules with all previous info (updates the speed[][] table)
reynolds_rules();
//printf("ROBOT %d: speed: %f, %f\n", robot_id, speed[robot_id][0], speed[robot_id][1]);
// Compute wheels speed from reynold's speed
compute_wheel_speeds(&msl, &msr);
//printf("wheels: %d, %d\n", msl, msr);
// Adapt speed instinct to distance sensor values
if (sum_sensors > NB_SENSORS*MIN_SENS) {
msl -= msl*max_sens/(2*MAX_SENS);
msr -= msr*max_sens/(2*MAX_SENS);
}
// Add Braitenberg
msl += bmsl;
msr += bmsr;
// Set speed
wb_differential_wheels_set_speed(msl,msr);
// Continue one step
wb_robot_step(TIME_STEP);
}
wb_emitter_send(emitter0,(void *)buffer,sizeof(double));
wb_receiver_next_packet(receiver0);
}
}
int main(int argc, char **argv) {
wb_robot_init();
// check if roslaunch is properly installed
if (system("which roslaunch > /dev/null")!=0) {
fprintf(stderr,"Cannot find roslaunch in PATH. Please check that ROS is properly installed.\n");
wb_robot_cleanup();
return 0;
}
// launch the joy ROS node
int roslaunch=fork();
if (roslaunch==0) { // child process
execlp("roslaunch","roslaunch","joy.launch",NULL);
return 0;
}
// From this point we assume the ROS_ROOT, ROS_MASTER_URI and
// ROS_PACKAGE_PATH environment variables are set appropriately.
// See ROS manuals to set them properly.
ros::init(argc, argv, "joystick");
ros::NodeHandle nh;
ros::Subscriber sub=nh.subscribe("joy", 10, joy_callback);
// ros::Subscriber sub=nh.subscribe<int32>("joy", 10, joy_callback);
double left_speed = 0;
double right_speed = 0;
ROS_INFO("Joypad connected and running.");
ROS_INFO("Commands:");
ROS_INFO("arrow up: move forward, speed up");
ROS_INFO("arrow down: move backward, slow down");
ROS_INFO("arrow left: turn left");
ROS_INFO("arrow right: turn right");
// control loop: simulation steps of 32 ms
while(wb_robot_step(32) != -1) {
// get callback called
ros::spinOnce();
switch(cmd) {
case JOYPAD_UP:
left_speed=SPEED;
right_speed=SPEED;
break;
case JOYPAD_DOWN:
left_speed=-SPEED;
right_speed=-SPEED;
break;
case JOYPAD_LEFT:
left_speed=-SPEED;
right_speed=SPEED;
break;
case JOYPAD_RIGHT:
left_speed=SPEED;
right_speed=-SPEED;
break;
case JOYPAD_STOP:
left_speed = 0;
right_speed = 0;
break;
}
// set motor speeds
wb_differential_wheels_set_speed(left_speed, right_speed);
}
ros::shutdown();
kill(roslaunch,SIGINT); // terminate roslaunch for joy ROS node as with Ctrl-C
wb_robot_cleanup();
return 0;
}
