/* Detects if a robot has handled an event (i.e. has bounced into it) */
bool are_colliding(WbFieldRef robTransRef, WbFieldRef eveTransRef) {
const double *p1 = wb_supervisor_field_get_sf_vec3f(robTransRef);
const double *p2 = wb_supervisor_field_get_sf_vec3f(eveTransRef);
double dx = p2[0] - p1[0];
double dz = p2[2] - p1[2];
// true if the distance between the center of the event and the center of the robots are closer than the sum of both radii
return sqrt(dx * dx + dz * dz) < 0.95 *EVENT_RADIUS + ROBOT_RADIUS;
}
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;
}
}
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;
}
/* Reset the supervisor */
void reset(void) {
int i;
char s[50];
char *eventprefix = (char *) "e";
printed = false;
for (i=0;i<ROBOTS;i++)
{
char aux[15];
/* Get and save a reference to the robot. */
sprintf(aux,"%s%d",rob_prefix,i+1);
// rob_name << rob_prefix << i+1; // << "_0";
rob[i] = wb_supervisor_node_get_from_def(aux);
wb_supervisor_field_set_sf_vec3f(wb_supervisor_node_get_field(rob[i],"translation"), initLoc[i]);
wb_supervisor_field_set_sf_rotation(wb_supervisor_node_get_field(rob[i],"rotation"), initRot[i]);
robTrans[i] = wb_supervisor_node_get_field(rob[i],"translation");
previous_x[i] = wb_supervisor_field_get_sf_vec3f(robTrans[i])[0];
previous_y[i] = wb_supervisor_field_get_sf_vec3f(robTrans[i])[2];
/* Get robot emitters */
strcpy(aux,emi_prefix);
sprintf(aux,"%s%d",aux,i+1);
}
printf("Initializing events...\n");
for (i=0; i<MAX_EVENTS; i++)
{
sprintf(s, "%s%d", eventprefix, i);
// Define position of event and place it there
events[i].event = wb_supervisor_node_get_from_def(s);
events[i].eventTrans = wb_supervisor_node_get_field(events[i].event,"translation");
events[i].state = 1.0;
randomize_event_position(i);
randomize_event_color(i);
}
}
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);
}
}
static void run_penalty_kick_shootout() {
show_message("PENALTY KICK SHOOT-OUT!");
step();
// power off robots and move them out of the field
int i;
for (i = 0; i < NUM_ROBOTS; i++) {
// keep RED_PLAYER_1 and BLUE_GOAL_KEEPER for the shootout
if (i != ATTACKER && i != GOALIE) {
// make them zombies
set_controller(i, "void");
if (robot_translation[i]) {
// move them out of the field but preserve elevation
double elevation = wb_supervisor_field_get_sf_vec3f(robot_translation[i])[Y];
double out_of_field[3] = { 1.0 * i, elevation, 5.0 };
wb_supervisor_field_set_sf_vec3f(robot_translation[i], out_of_field);
}
}
}
// inform robots of the penalty kick shootout
control_data.secondaryState = STATE2_PENALTYSHOOT;
// five penalty shots per team
for (i = 0; i < 5; i++) {
run_penalty_kick(60);
if (check_victory(5 - i)) return;
run_finished_state();
swap_teams_and_scores();
run_penalty_kick(60);
if (check_victory(4 - i)) return;
run_finished_state();
swap_teams_and_scores();
}
if (match_type == FINAL) {
show_message("SUDDEN DEATH SHOOT-OUT!");
// sudden death shots
while (1) {
run_penalty_kick(120);
run_finished_state();
swap_teams_and_scores();
run_penalty_kick(120);
if (check_victory(0)) return;
run_finished_state();
swap_teams_and_scores();
}
}
}
// 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();
}
// 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() {
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, 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;
}
}
static int run(int ms) {
static unsigned long long int clock = 0;
static unsigned long long int temp_clock = 0;
int i, j;
// Computing the distance travelled by the robots
for (i = 0; i < ROBOTS; i++) {
const double *pos = wb_supervisor_field_get_sf_vec3f(robTrans[i]);
distance_travelled[i] += sqrt(pow(previous_x[i]-pos[0],2) + pow(previous_y[i] - pos[2], 2));
previous_x[i] = pos[0];
previous_y[i] = pos[2];
}
// Check if iteration over
if (temp_clock > MAX_TIME) {
// long long int duration = (clock - temp_clock) / 1000; // Duration in seconds
long long int duration = MAX_TIME / 1000; // Duration in seconds
// Events per unit of time
double metric1 = ((double) events_handled)/((double)duration); // Compute performance
// Total distance travelled per unit of time
double total_distance = 0;
for (i = 0; i < ROBOTS; i++) {
total_distance += distance_travelled[i];
distance_travelled[i] = 0;
}
double metric2 = total_distance / events_handled;
//mean_perf1 += metric1/MAX_DURATION;
//mean_perf2 += metric2/MAX_DURATION;
fprintf(outfile, "metric1(%d) = %f;\n", iterations, metric1);
fprintf(outfile, "metric2(%d) = %f;\n", iterations, metric2);
printf("Robots completed %d tasks in %llis, performance = %f \n", events_handled, duration, metric1);
printf("Robots travelled %f meters in %llis, performance = %f \n", total_distance, duration, metric2);
iterations++;
sleep(1);
events_handled = 0;
//temp_clock = clock;
temp_clock = 0;
reset();
}
// Stop simulating after a certain number of iterations
if (iterations > MAX_DURATION) {
//printf("Simulation terminated in %llis. Mean speed = %f. Mean distance per event = %f\n", clock/1000, mean_perf1, mean_perf2);
sleep(5);
fclose(outfile);
while(true);
}
/* Get data */
for (i=0;i<ROBOTS;i++) {
/* Test if a robot has bounced in an obstacle */
for (j=0;j<MAX_EVENTS;j++){
bool collision = are_colliding(robTrans[i], events[j].eventTrans);
/* Update the number of events and remove/replace the handled event*/
if(collision){
events[j].state -= EVENT_PRODUCTIVITY_STEP;
if (events[j].state<=0) {
// reset event
events[j].state = 1.0;
randomize_event_position(j);
randomize_event_color(j);
events_handled++;
//printf("%d events handled \n", events_handled);
}
}
}
}
clock+=STEP_SIZE;
temp_clock+=STEP_SIZE;
return STEP_SIZE;
}
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;
}
/*
* This is the main program.
* The arguments of the main function can be specified by the
* "controllerArgs" field of the Robot node
*/
int main(int argc, char **argv)
{
//int cont=0;
using_shared_memory();
/* necessary to initialize webots stuff */
wb_robot_init();
// do this once only Darwin
WbNodeRef robot_node = wb_supervisor_node_get_from_def("Darwin");
WbFieldRef trans_field = wb_supervisor_node_get_field(robot_node, "translation");
WbFieldRef rot_field = wb_supervisor_node_get_field(robot_node, "rotation");
int caiu_cont = 0;
// do this once only Darwin2
WbNodeRef robot_node_2 = wb_supervisor_node_get_from_def("Darwin2");
WbFieldRef trans_field_2 = wb_supervisor_node_get_field(robot_node_2, "translation");
WbFieldRef rot_field_2 = wb_supervisor_node_get_field(robot_node_2, "rotation");
// do this once only Darwin3
WbNodeRef robot_node_3 = wb_supervisor_node_get_from_def("Darwin3");
WbFieldRef trans_field_3 = wb_supervisor_node_get_field(robot_node_3, "translation");
WbFieldRef rot_field_3 = wb_supervisor_node_get_field(robot_node_3, "rotation");
while (1) {
// this is done repeatedly
const double *trans = wb_supervisor_field_get_sf_vec3f(trans_field);
const double *trans_2 = wb_supervisor_field_get_sf_vec3f(trans_field_2);
const double *trans_3 = wb_supervisor_field_get_sf_vec3f(trans_field_3);
TRANS1 = trans[0];
TRANS2 = trans[1];
TRANS3 = trans[2];
TRANS1_2 = trans_2[0];
TRANS2_2 = trans_2[1];
TRANS3_2 = trans_2[2];
TRANS1_3 = trans_3[0];
TRANS2_3 = trans_3[1];
TRANS3_3 = trans_3[2];
//printf("MY_ROBOT is at position: %g %g %g\n", trans[0], trans[1], trans[2]);
wb_robot_step(32);
if(RESET_ROBOT == 1)
{
caiu_cont++;
CAIU_CONT = caiu_cont;
// reset robot position
const double INITIAL[3] = { 0, 0.32004, 0 };
const double INITIAL_ROT[4] = { 0.211189, 0.971678, -0.106025, 0.944968 };
wb_supervisor_field_set_sf_vec3f(trans_field, INITIAL);
wb_supervisor_field_set_sf_rotation(rot_field, INITIAL_ROT);
//wb_supervisor_simulation_reset_physics();
RESET_ROBOT = 0;
if(caiu_cont > 25)
{
caiu_cont = 0;
wb_supervisor_simulation_revert(); // restart the simulation
// A medida que o tempo vai passando o robo Darwin vai ficando
// cada vez mais devagar como se houvesse algum tipo de desgaste fisico
//entao e necessario restarta a simulaçao
}
}
if(RESET_ROBOT_2 == 1)
{
// reset robot position
const double INITIAL_2[3] = { 0.2, 0.32004, 4.7 };
const double INITIAL_ROT_2[4] = { 0.211189, 0.971678, -0.106025, 0.944968 };
wb_supervisor_field_set_sf_vec3f(trans_field_2, INITIAL_2);
wb_supervisor_field_set_sf_rotation(rot_field_2, INITIAL_ROT_2);
//wb_supervisor_simulation_reset_physics();
RESET_ROBOT_2 = 0;
}
if(RESET_ROBOT_3 == 1)
{
// reset robot position
const double INITIAL_3[3] = { 3.2, 0.32004, -3.7 };
const double INITIAL_ROT_3[4] = { 0.211189, 0.971678, -0.106025, 0.944968 };
wb_supervisor_field_set_sf_vec3f(trans_field_3, INITIAL_3);
wb_supervisor_field_set_sf_rotation(rot_field_3, INITIAL_ROT_3);
//wb_supervisor_simulation_reset_physics();
RESET_ROBOT_3 = 0;
}
}
/*
* 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");
*/
/* This is necessary to cleanup webots resources */
wb_robot_cleanup();
return 0;
}
