Αναφορά εργασίας. Χατζητζάνος Ευάγγελος

Transcript

1 Αναφορά εργασίας Χατζητζάνος Ευάγγελος Η εργασία εξαμήνου είναι στο περιβάλλον του Webots και περιλαμβάνει το Nao, καθώς και σκαλοπάτια που πρέπει να ανεβαίνει. Αρχικά έχουμε τον έτοιμο κώδικα για το Nao, το nao_demo.c και τροποποιήθηκε έτσι ώστε να κάνει τις απαραίτητες ενέργειες. Ο κώδικας αυτός δίνει τη δυνατότητα στο χρήστη να χειρίζεται το ρομπότ μέσω των πλήκτρων καθως και στο να βλέπει τιμές των bumpers και άλλων μεταβλητών. Η τροποποίηση του κώδικα περιλαμβάνει τη σύνταξη κώδικα, και συγκεκριμένα τη δημιουργία κατάλληλης συνάρτησης προκειμένου να σταματάει το Nao ακριβώς τη στιγμή που ανιχνεύουν τα bumpers των ποδιών κάποιο εμπόδιο. Στην προκειμένη περίπτωση το εμπόδιο είναι το σκαλοπάτι. Η συνάρτηση είναι η εξής: static int stair_up_function(key,stair,ll,lr,rl,rr){ int i=0; do { wbu_motion_play(forwards); simulation_step(); i=i+1; if(i>=30){ ll = (int)wb_touch_sensor_get_value(lfoot_lbumper); lr = (int)wb_touch_sensor_get_value(lfoot_rbumper); rl = (int)wb_touch_sensor_get_value(rfoot_lbumper); rr = (int)wb_touch_sensor_get_value(rfoot_rbumper); else{ ll=0; lr=0; rl=0;

2 rr=0; key = wb_robot_keyboard_get_key(); while ( (! key) && (( ll==0) && (lr==0) && (rl==0 ) && (rr==0)) ); // stop looping this motion wbu_motion_set_loop(forwards, false); wbu_motion_stop(forwards); wbu_motion_play(stair_up); stair=stair+1; return stair; Στη συνέχεια έπρεπε μέσω του Motion Editor να υλοποιηθεί η κίνηση του Nao για να ανέβει το σκαλοπάτι. Αυτό απαιτεί την ισορροπία του Nao για να ολοκληρωθεί η κίνηση, μέσω κατάλληλων και πολύ μικρών (μεταξύ κάθε 2 pose) αλλαγών στις αρθρώσεις του, για να έχουμε το ζητούμενο αποτέλεσμα. Επίσης χρειάστηκε να συνταχθεί κώδικας για να εκτελεί τις παραπάνω διαδικασίες το Nao επαναληπτικά, δηλαδή έως ότου ανέβει στο τρίτο, και τελευταίο, σκαλοπάτι. Η συνάρτηση main είναι η εξής: int main(int argc, const char *argv[]) { // call this before any other call to a Webots function wb_robot_init(); // simulation step in milliseconds time_step = wb_robot_get_basic_time_step(); // initialize stuff find_and_enable_devices();

3 load_motion_files(); // move shoulder motors down wb_servo_set_position(rshoulderpitch, 1.4); wb_servo_set_position(lshoulderpitch, 1.4); // print instructions print_help(); // walk forwards until a key is pressed wbu_motion_set_loop(forwards, true); int key = 0 lfoot_lbumper = wb_robot_get_device("bumperlfootleft"); lfoot_rbumper = wb_robot_get_device("bumperlfootright"); rfoot_lbumper = wb_robot_get_device("bumperrfootleft"); rfoot_rbumper = wb_robot_get_device("bumperrfootright"); wb_touch_sensor_enable(lfoot_lbumper, time_step); wb_touch_sensor_enable(lfoot_rbumper, time_step); wb_touch_sensor_enable(rfoot_lbumper, time_step); wb_touch_sensor_enable(rfoot_rbumper, time_step); int ll = -1; int lr = -1; int rl = -1; int rr = -1; //int i=0; int stair = 0;

4 stair_up_function(key,stair,ll,lr,rl,rr); if (stair==1){ stair_up_function(key,stair,ll,lr,rl,rr); if (stair==2){ stair_up_function(key,stair,ll,lr,rl,rr); // read keyboard and execute user commands while (1) { if (key) run_command(key,ll,lr,rl,rr); simulation_step(); key = wb_robot_keyboard_get_key(); return 0;

5 Οι δηλώσεις των μεταβλητών και οι υπόλοιπες συναρτήσεις είναι οι εξής: #include <webots/utils/motion.h> #include <webots/robot.h> #include <webots/servo.h> #include <webots/camera.h> #include <webots/distance_sensor.h> #include <webots/accelerometer.h> #include <webots/gyro.h> #include <webots/touch_sensor.h> #include <webots/led.h> #include <string.h> #include <stdio.h> #include <stdlib.h> #include <unistd.h> #ifdef _MSC_VER #define snprintf sprintf_s #endif #define PHALANX_MAX 8 static int time_step = -1; // simulated devices static WbDeviceTag camera, camera_select; // camera with high/low position

6 static WbDeviceTag us[4]; static WbDeviceTag accelerometer, gyro; static WbDeviceTag fsr[2][4]; // ultra sound sensors // inertial unit // force sensitive resistors static WbDeviceTag lfoot_lbumper, lfoot_rbumper; static WbDeviceTag rfoot_lbumper, rfoot_rbumper; // left foot bumpers // right foot bumpers static WbDeviceTag leds[7]; lphalanx[phalanx_max]; // controllable led groupsstatic WbDeviceTag static WbDeviceTag rphalanx[phalanx_max]; static WbDeviceTag lphalanx[phalanx_max]; // right hand motors // left hand motors static WbDeviceTag RShoulderPitch; static WbDeviceTag LShoulderPitch; // motion file handles static WbMotionRef stand_up, hand_wave, forwards, backwards, side_step_left, side_step_right, turn_left_60, turn_right_60, stair_up; static WbMotionRef currently_playing = NULL; static void find_and_enable_devices() { // camera camera = wb_robot_get_device("camera"); wb_camera_enable(camera, 4 * time_step); // camera selection (high/low) camera_select = wb_robot_get_device("cameraselect");

7 // accelerometer accelerometer = wb_robot_get_device("accelerometer"); wb_accelerometer_enable(accelerometer, time_step); // gyro gyro = wb_robot_get_device("gyro"); wb_gyro_enable(gyro, time_step); // ultrasound sensors us[0] = wb_robot_get_device("ussensor1"); us[1] = wb_robot_get_device("ussensor2"); us[2] = wb_robot_get_device("ussensor3"); us[3] = wb_robot_get_device("ussensor4"); int i; for (i = 0; i < 4; i++) wb_distance_sensor_enable(us[i], time_step); // foot sensors fsr[0][0] = wb_robot_get_device("lfsrfl"); fsr[0][1] = wb_robot_get_device("lfsrfr"); fsr[0][2] = wb_robot_get_device("lfsrrr"); fsr[0][3] = wb_robot_get_device("lfsrrl"); fsr[1][0] = wb_robot_get_device("rfsrfl"); fsr[1][1] = wb_robot_get_device("rfsrfr"); fsr[1][2] = wb_robot_get_device("rfsrrr"); fsr[1][3] = wb_robot_get_device("rfsrrl"); for (i = 0; i < 4; i++) {

8 wb_touch_sensor_enable(fsr[0][i], time_step); wb_touch_sensor_enable(fsr[1][i], time_step); // foot bumpers /*lfoot_lbumper = wb_robot_get_device("bumperlfootleft"); lfoot_rbumper = wb_robot_get_device("bumperlfootright"); rfoot_lbumper = wb_robot_get_device("bumperrfootleft"); rfoot_rbumper = wb_robot_get_device("bumperrfootright"); wb_touch_sensor_enable(lfoot_lbumper, time_step); wb_touch_sensor_enable(lfoot_rbumper, time_step); wb_touch_sensor_enable(rfoot_lbumper, time_step); wb_touch_sensor_enable(rfoot_rbumper, time_step);*/ // There are 7 controlable LED groups in Webots leds[0] = wb_robot_get_device("chestboard/led"); leds[1] = wb_robot_get_device("rfoot/led"); leds[2] = wb_robot_get_device("lfoot/led"); leds[3] = wb_robot_get_device("face/led/right"); leds[4] = wb_robot_get_device("face/led/left"); leds[5] = wb_robot_get_device("ears/led/right"); leds[6] = wb_robot_get_device("ears/led/left"); // get phalanx motor tags // the real Nao has only 2 motors for RHand/LHand // but in Webots we must implement RHand/LHand with 2x8 motors for (i = 0; i < PHALANX_MAX; i++) {

9 char name[32]; sprintf(name, "LPhalanx%d", i + 1); lphalanx[i] = wb_robot_get_device(name); sprintf(name, "RPhalanx%d", i + 1); rphalanx[i] = wb_robot_get_device(name); // shoulder pitch motors RShoulderPitch = wb_robot_get_device("rshoulderpitch"); LShoulderPitch = wb_robot_get_device("lshoulderpitch"); // keyboard wb_robot_keyboard_enable(10 * time_step); // load motion files static void load_motion_files() { hand_wave = wbu_motion_new("../../motions/handwave.motion"); forwards = wbu_motion_new("../../motions/forwards50.motion"); backwards = wbu_motion_new("../../motions/backwards.motion"); side_step_left = wbu_motion_new("../../motions/sidestepleft.motion"); side_step_right = wbu_motion_new("../../motions/sidestepright.motion"); turn_left_60 = wbu_motion_new("../../motions/turnleft60.motion"); turn_right_60 = wbu_motion_new("../../motions/turnright60.motion"); stair_up = wbu_motion_new("../../motions/stair_up7_3.motion"); stand_up = wbu_motion_new("../../motions/standupfromfront.motion"); isiose = wbu_motion_new("../../motions/isiose.motion");

10 static void start_motion(wbmotionref motion) { // interrupt current motion if (currently_playing) wbu_motion_stop(currently_playing); // start new motion wbu_motion_play(motion); currently_playing = motion; // the accelerometer axes are oriented as on the real robot // however the sign of the returned values may be opposite static void print_acceleration() { const double *acc = wb_accelerometer_get_values(accelerometer); printf(" accelerometer \n"); printf("acceleration: [ x y z ] = [%f %f %f]\n", acc[0], acc[1], acc[2]); // the gyro axes are oriented as on the real robot // however the sign of the returned values may be opposite static void print_gyro() { const double *vel = wb_gyro_get_values(gyro); printf(" gyro \n"); printf("angular velocity: [ x y ] = [%f %f]\n", vel[0], vel[1]);

11 static void print_foot_sensors() { double newtons = 0.0; double fsv[2][4]; // force sensor values int i; for (i = 0; i < 4; i++) { fsv[0][i] = wb_touch_sensor_get_value(fsr[0][i]); fsv[1][i] = wb_touch_sensor_get_value(fsr[1][i]); newtons += fsv[0][i] + fsv[1][i]; printf(" foot sensors \n"); printf(" left right\n"); printf(" \n"); printf(" %3.1f %3.1f %3.1f %3.1f front\n", fsv[0][0], fsv[0][1], fsv[1][0], fsv[1][1]); printf(" \n"); printf(" %3.1f %3.1f %3.1f %3.1f back\n", fsv[0][3], fsv[0][2], fsv[1][3], fsv[1][2]); printf(" \n"); printf("total: %g Newtons, %g kilograms\n", newtons, newtons / 9.81); /*static void print_foot_bumpers() { int ll = (int)wb_touch_sensor_get_value(lfoot_lbumper); int lr = (int)wb_touch_sensor_get_value(lfoot_rbumper); int rl = (int)wb_touch_sensor_get_value(rfoot_lbumper); int rr = (int)wb_touch_sensor_get_value(rfoot_rbumper);

12 printf(" foot bumpers \n"); printf(" left right\n"); printf(" \n"); printf(" %d %d %d %d \n", ll, lr, rl, rr); printf(" \n"); printf(" \n"); printf(" \n"); */ static void print_ultrasound_sensors() { double dist[4]; int i; for (i = 0; i < 4; i++) dist[i] = wb_distance_sensor_get_value(us[i]); printf("-----ultrasound sensors-----\n"); printf("top: left: %f m, right %f m\n", dist[2], dist[0]); printf("bottom left: %f m, right %f m\n", dist[3], dist[1]); static void print_camera_image() { const int SCALED = 2; int width = wb_camera_get_width(camera); int height = wb_camera_get_height(camera);

13 // read rgb pixel values from the camera const unsigned char *image = wb_camera_get_image(camera); printf(" camera image (grey levels) \n"); printf("original resolution: %d x %d, scaled to %d x %d\n", width, height, width / SCALED, height / SCALED); int y, x; char *line = malloc(width / SCALED + 1); line[width / SCALED] = 0; // add line termination for (y = 0; y < height; y += SCALED) { int count = 0; for (x = 0; x < width; x += SCALED) { unsigned char grey = wb_camera_image_get_grey(image, width, x, y); line[count++] = '0' + grey * 9 / 255; line[count++] = 0; printf("%s\n", line); free(line); static void set_all_leds_color(int rgb) { // these leds take RGB values int i;

14 for (i = 0; i < 5; i++) wb_led_set(leds[i], rgb); // ear leds are single color (blue) // and take values between wb_led_set(leds[5], rgb & 0xff); wb_led_set(leds[6], rgb & 0xff); static void set_hands_angle(double angle) { // we must activate the 8 phalanx motors int j; for (j = 0; j < PHALANX_MAX; j++) { if (rphalanx[j]) wb_servo_set_position(rphalanx[j], angle); if (lphalanx[j]) wb_servo_set_position(lphalanx[j], angle); static void print_help() { printf(" nao_demo \n"); printf("use the keyboard to control the robot\n"); printf("(the 3D window need to be focused)\n"); printf("[up][down]: move one step forward/backwards\n"); printf("[<-][->]: side step left/right\n"); printf("[shift] + [<-][->]: turn left/right\n");

15 printf("[u]: print ultrasound sensors\n"); printf("[a]: print accelerometers\n"); printf("[g]: print gyros\n"); printf("[f]: print foot sensors\n"); printf("[b]: print foot bumpers\n"); printf("[c]: print scaled camera image\n"); printf("[home][end]: camera selection (high/low)\n"); printf("[pageup][pagedown]: open/close hands\n"); printf("[7][8][9]: change all leds RGB color\n"); printf("[0]: turn all leds off\n"); printf("[h]: print this help message\n"); static void terminate() { // add you cleanup code here: write results, close files, free memory, etc. //... wb_robot_cleanup(); static void simulation_step() { if (wb_robot_step(time_step) == -1) terminate(); static void run_command(int key,int ll, int lr,int rl, int rr) {

16 switch (key) { case WB_ROBOT_KEYBOARD_LEFT: start_motion(side_step_left); case WB_ROBOT_KEYBOARD_RIGHT: start_motion(side_step_right); case WB_ROBOT_KEYBOARD_UP: wbu_motion_set_loop(forwards, true); do { ll = (int)wb_touch_sensor_get_value(lfoot_lbumper); lr = (int)wb_touch_sensor_get_value(lfoot_rbumper); rl = (int)wb_touch_sensor_get_value(rfoot_lbumper); rr = (int)wb_touch_sensor_get_value(rfoot_rbumper); wbu_motion_play(forwards); simulation_step(); while (( ll==0) && (lr==0) && (rl==0 ) && (rr==0)); wbu_motion_set_loop(forwards, false); wbu_motion_stop(forwards); start_motion(backwards);

17 case WB_ROBOT_KEYBOARD_DOWN: start_motion(backwards); case WB_ROBOT_KEYBOARD_LEFT WB_ROBOT_KEYBOARD_SHIFT: start_motion(turn_left_60); case WB_ROBOT_KEYBOARD_RIGHT WB_ROBOT_KEYBOARD_SHIFT: start_motion(turn_right_60); case 'A': print_acceleration(); case 'G': print_gyro(); case 'F': print_foot_sensors(); case 'B': //print_foot_bumpers(); case 'U': print_ultrasound_sensors(); case 'C': print_camera_image();

18 case WB_ROBOT_KEYBOARD_HOME: wb_servo_set_position(camera_select, 0); case WB_ROBOT_KEYBOARD_END: wb_servo_set_position(camera_select, 1); case WB_ROBOT_KEYBOARD_PAGEUP: set_hands_angle(1.0); case WB_ROBOT_KEYBOARD_PAGEDOWN: set_hands_angle(0.0); case '7': set_all_leds_color(0xff0000); // red case '8': set_all_leds_color(0x00ff00); // green case '9': set_all_leds_color(0x0000ff); // blue case '0': set_all_leds_color(0x000000); // off case 'H': print_help();

19