/examples/meka/src/mekajointinterface.cpp - Annotate - humotion - Research for Cognitive Interaction

humotion / examples / meka / src / mekajointinterface.cpp @ 99ebae32

History | View | Annotate | Download (30.423 KB)

-be6243f
+#include "mekajointinterface.h"
 using namespace std;
 //WARNING: DO NOT CHANGE THIS; VELOCITYMODE IS NOT YET IMPLEMENTED
 #define POSITION_CONTROL 1
-a6a6c
+void MekaJointInterface::incoming_jointstates(const sensor_msgs::JointState & msg){
     //joint_pos_global = msg.position[3];
     cout << msg;
+}
-be6243f
+//! constructor
 MekaJointInterface::MekaJointInterface(string _scope) : humotion::server::JointInterface(){
     scope = _scope;
-a6a6c
+    //subscribe to meka joint states
-ebae32
+    int argc = 0;
     ros::init(argc, (char**)NULL, "meka_humotion");
-a6a6c
+    ros::NodeHandle n;
     joint_state_subscriber = n.subscribe(scope + "joint_states", 150, &MekaJointInterface::incoming_jointstates , this);
-be6243f
+    //add mapping from ids to enums:
     //this might look strange at the first sight but we need to have a generic
     //way to acces joints from libhumotion. therefore the lib uses its enum with ID_* enum ids
     //to access the joints. now we need to define a mapping to map those to our motor ids.
     //this is what we use the enum bimap for (convertion fro/to motorid is handled
     //by \sa convert_enum_to_motorid() and \sa convert_motorid_to_enum() lateron
 /*
     //MOUTH
     enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_LIP_LEFT_UPPER,   ID_LIP_LEFT_UPPER));
     enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_LIP_LEFT_LOWER,   ID_LIP_LEFT_LOWER));
     enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_LIP_CENTER_UPPER, ID_LIP_CENTER_UPPER));
     enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_LIP_CENTER_LOWER, ID_LIP_CENTER_LOWER));
     enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_LIP_RIGHT_UPPER,  ID_LIP_RIGHT_UPPER));
     enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_LIP_RIGHT_LOWER,  ID_LIP_RIGHT_LOWER));
     //NECK
     enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_NECK_PAN,    ID_NECK_PAN));
     enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_NECK_TILT,   ID_NECK_TILT));
     enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_NECK_ROLL,   ID_NECK_ROLL));
     //EYES
     enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_LEFT_LR,   ID_EYES_LEFT_LR));
     enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_RIGHT_LR,   ID_EYES_RIGHT_LR));
     enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_BOTH_UD,   ID_EYES_BOTH_UD));
     //EYELIDS
     enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_LEFT_LID_LOWER, ID_EYES_LEFT_LID_LOWER));
     enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_LEFT_LID_UPPER, ID_EYES_LEFT_LID_UPPER));
     enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_LEFT_BROW, ID_EYES_LEFT_BROW));
     enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_RIGHT_LID_LOWER, ID_EYES_RIGHT_LID_LOWER));
     enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_RIGHT_LID_UPPER,ID_EYES_RIGHT_LID_UPPER));
     enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_RIGHT_BROW, ID_EYES_RIGHT_BROW));
     Property options;
     options.put("device", "remote_controlboard");
     options.put("local", "/local/head");
     options.put("remote", scope+"/head");
     dd.open(options);
     //fetch views:
     dd.view(iencs);
     dd.view(ipos);
     dd.view(ivel);
     dd.view(ilimits);
     if ( (!iencs) || (!ipos) || (!ilimits) || (!ivel)){
         printf("> ERROR: failed to open icub views\n");
         exit(EXIT_FAILURE);
+    }
     int joints;
     //tell humotion about min/max joint values:
     init_joints();
     iencs->getAxes(&joints);
     positions.resize(joints);
     velocities.resize(joints);
     commands.resize(joints);
     //set position mode:
     if (POSITION_CONTROL){
         commands=200000.0;
         ipos->setRefAccelerations(commands.data());
         ipos->setPositionMode();
     }else{
         ivel->setVelocityMode();
         commands=1000000;
         ivel->setRefAccelerations(commands.data());
+    }
     //attach to facial expressions:
     string emotion_scope = scope + "/face/raw/in";
     printf("> opening connection to %s\n", emotion_scope.c_str());
     for(int i=0; i<4; i++){
         //strange, if we use one output port only the first command is executed?! flushing issues?
         string emotion_port_out = "/emotionwriter" + to_string(i);
         if (!emotion_port[i].open(emotion_port_out.c_str())){
             printf("> ERROR: failed to open to %s\n",emotion_port_out.c_str());
             exit(EXIT_FAILURE);
+        }
         if (!Network::connect(emotion_port_out.c_str(), emotion_scope.c_str())){
             printf("> ERROR: failed to connect emotion ports\n");
             exit(EXIT_FAILURE);
+        }
+    }
     */
+}
 //! destructor
 MekaJointInterface::~MekaJointInterface(){
+}
-a6a6c
+            Simon Schulz
 void MekaJointInterface::run(){
    //iCubDataReceiver *data_receiver = new iCubDataReceiver(10/*1000.0 / MAIN_LOOP_FREQUENCY*/, iencs, this);
    //data_receiver->start();
+}
 //! set the target position of a joint
 //! \param enum id of joint
 //! \param float value
 void MekaJointInterface::publish_target_position(int e){
     #if 0
     //first: convert humotion enum to our enum:
     int id = convert_enum_to_motorid(e);
     if (id == -1){
         return; //we are not interested in that data, so we just return here
+    }
     if (id == ICUB_ID_NECK_PAN){
         //PAN seems to be swapped
         store_joint(ICUB_ID_NECK_PAN, -joint_target[e]);
     }else if ((id == ICUB_ID_EYES_LEFT_LR) || ( id == ICUB_ID_EYES_RIGHT_LR)){
         //icub handles eyes differently, we have to set pan angle + vergence
         float pan      = (joint_target[ID_EYES_LEFT_LR] + joint_target[ID_EYES_RIGHT_LR]) / 2;
         float vergence = (joint_target[ID_EYES_LEFT_LR]  - joint_target[ID_EYES_RIGHT_LR]);
         //printf("EYEDBG %3.2f %3.2f --_> pan %3.2f verg=%3.2f\n",joint_target[ID_EYES_LEFT_LR], joint_target[ID_EYES_RIGHT_LR],pan,vergence);
         store_joint(ICUB_ID_EYES_PAN, pan);
         store_joint(ICUB_ID_EYES_VERGENCE, vergence);
     }else{
         store_joint(id, joint_target[e]);
+    }
     #endif
+}
 //! actually execute the scheduled motion commands
 void MekaJointInterface::execute_motion(){
     #if 0
     // set up neck and eye motion commands:
     if (POSITION_CONTROL){
         //position control
         for(int i=ICUB_ID_NECK_TILT; i<=ICUB_ID_EYES_VERGENCE; i++){
             set_target_in_positionmode(i, target_angle[i]);
+        }
     }else{
         //velocity control
         for(int i=ICUB_ID_NECK_TILT; i<=ICUB_ID_EYES_VERGENCE; i++){
             set_target_in_velocitymode(i, target_angle[i]);
+        }
+    }
     //printf("> TARGET PAN = %3.2f\n",target_angle[ICUB_ID_NECK_PAN]);
     //eyelids: unfortuantely the icub has only 1dof for eyelids, so we use only one dof here:
     set_eyelid_angle(target_angle[ICUB_ID_EYES_RIGHT_LID_UPPER]);
     //eyebrows are set using a special command as well:
     set_eyebrow_angle(ICUB_ID_EYES_LEFT_BROW);
     set_eyebrow_angle(ICUB_ID_EYES_RIGHT_BROW);
     //mouth
     set_mouth();
     #endif
+}
 //! prepare and enable a joint
 //! NOTE: this should also prefill the min/max positions for this joint
 //! \param the enum id of a joint
 void MekaJointInterface::enable_joint(int e){
 #if 0
     //FIXME ADD THIS:
     // enable the amplifier and the pid controller on each joint
     /*for (i = 0; i < nj; i++) {
        amp->enableAmp(i);
        pid->enablePid(i);
     }*/
     //set up smooth motion controller
     //step1: set up framerate
     //dev->set_register_blocking(XSC3_REGISTER_PID_RAMP, framerate, true);
     //step2: set controllertype:
     //printf("> activating smooth motion control for joint id 0x%02X (%s)\n", motor_id, joint_name.c_str());
     //dev->set_register_blocking(XSC3_REGISTER_PID_CONTROLLER, XSC3_PROTOCOL_PID_CONTROLLERTYPE_SMOOTH_PLAYBACK, true);
     //step3: set pid controller:
     /*if ((e == ID_LIP_LEFT_UPPER) || (e == ID_LIP_LEFT_LOWER) || (e == ID_LIP_CENTER_UPPER) || (e == ID_LIP_CENTER_LOWER) || (e == ID_LIP_RIGHT_UPPER) || (e == ID_LIP_RIGHT_LOWER)){
         printf("> fixing PID i controller value for smooth motion (FIXME: restore old value!!)\n");
         dev->set_register_blocking(XSC3_REGISTER_CONST_I, 10, true);
     }*/
     //uint16_t result = dev->get_register_blocking_raw(XSC3_REGISTER_PID_CONTROLLER);
     //check if setting pid controllertype was successfull:
     /*if (result != XSC3_PROTOCOL_PID_CONTROLLERTYPE_SMOOTH_PLAYBACK){
         printf("> failed to set smooth motion controller for joint %s (res=0x%04X)\n",joint_name.c_str(),result);
         exit(1);
     }*/
     //fetch min/max:
    // init_joint(e);
     //ok fine, now enable motor:
     //printf("> enabling motor %s\n", joint_name.c_str());
     //dev->set_register_blocking(XSC3_REGISTER_STATUS, XSC3_PROTOCOL_STATUS_BITMASK_MOTOR_ENABLE, true);
 #endif
+}
 //! shutdown and disable a joint
 //! \param the enum id of a joint
 void MekaJointInterface::disable_joint(int e){
     /*
         //first: convert humotion enum to our enum:
         int motor_id = convert_enum_to_motorid(e);
         if (motor_id == -1){
             return; //we are not interested in that data, so we just return here
+        }
         //fetch device:
         Device *dev = get_device(motor_id);
         printf("> FIXME: ADD DISABLE CODE\n");
         printf("> FIXME: ADD DISABLE CODE\n");
         printf("> FIXME: ADD DISABLE CODE\n");
         printf("> FIXME: ADD DISABLE CODE\n");
         printf("> FIXME: ADD DISABLE CODE\n");
         printf("> FIXME: ADD DISABLE CODE\n");
         printf("> FIXME: ADD DISABLE CODE\n");
         printf("> FIXME: ADD DISABLE CODE\n");
         printf("> FIXME: ADD DISABLE CODE\n");
         */
+}
 #if 0
-be6243f
+            Sebastian Meyer zu Borgsen
 //! conversion table for humotion motor ids to our ids:
 //! \param enum from JointInterface::JOINT_ID_ENUM
 //! \return int value of motor id
 int iCubJointInterface::convert_enum_to_motorid(int e){
     enum_id_bimap_t::right_const_iterator it = enum_id_bimap.right.find(e);
     if(it == enum_id_bimap.right.end()) {
         //key does not exists, we are not interested in that dataset, return -1
         return -1;
+    }
     return it->second;
+}
 //! conversion table for our ids to humotion motor ids:
 //! \param  int value of motor id
 //! \return enum from JointInterface::JOINT_ID_ENUM
 int iCubJointInterface::convert_motorid_to_enum(int id){
     enum_id_bimap_t::left_const_iterator it = enum_id_bimap.left.find(id);
     if(it == enum_id_bimap.left.end()) {
         //key does not exists, we are not interested in that dataset, return -1
         return -1;
+    }
     return it->second;
+}
 //! special command to set eyelid angle
 //! \param angle in degrees
 void iCubJointInterface::set_eyelid_angle(double angle){
     if (emotion_port[0].getOutputCount()>0){
         //try to set the value based on the upper one
         //some guesses from the sim: S30 = 0° / S40 = 10°
         int opening = (25.0 + 0.8*angle);
         opening = min(48, max(24, opening));
         if (opening == lid_opening_previous){
             //no update necessary
             return;
+        }
         lid_angle = angle;
         lid_opening_previous = opening;
         char buf[20];
         sprintf(buf, "S%2d", opening);
         //printf("> SETTING EYELID '%s' (%f -> %d)\n",buf,angle,opening);
         Bottle &cmd = emotion_port[0].prepare();
         cmd.clear();
         cmd.addString(buf);
         emotion_port[0].write();
     }else{
         printf("> ERROR: no icub emotion output\n");
         exit(EXIT_FAILURE);
+    }
+}
 //! special command to set the eyebrow angle
 //! \param id {0=left, 1=right)
 //! \param angle in degrees
 void iCubJointInterface::set_eyebrow_angle(int id){
     int port_id;
     if (id == ICUB_ID_EYES_LEFT_BROW){
         port_id = 1;
     }else{
         port_id = 2;
+    }
     if (emotion_port[port_id].getOutputCount()>0){
         double angle = target_angle[id];
         int icub_val = 0;
         //swap rotation direction:
         if (id==ICUB_ID_EYES_LEFT_BROW) angle = -angle;
         //convert to icub representation
         if (angle < -20){
             icub_val = 1;
         }else if (angle<10){
             icub_val = 2;
         }else if (angle<20){
             icub_val = 4;
         }else{
             icub_val = 8;
+        }
         //make sure to update only on new values:
         if (icub_val == target_angle_previous[id]){
                 //no updata necessary
                 return;
+        }
         //store actual value:
         target_angle_previous[id] = icub_val;
         string cmd_s;
         if (id==ICUB_ID_EYES_LEFT_BROW){
             cmd_s = "L0" + to_string(icub_val);
         }else{
             cmd_s = "R0" + to_string(icub_val);
+        }
         printf("> SETTING EYEBROW %d (%f -> %s)\n",id,angle,cmd_s.c_str());
         Bottle &cmd = emotion_port[port_id].prepare();
         cmd.clear();
         cmd.addString(cmd_s);
         emotion_port[port_id].write();
     }else{
         printf("> ERROR: no icub emotion output\n");
         exit(EXIT_FAILURE);
+    }
+}
 void iCubJointInterface::run(){
     iCubDataReceiver *data_receiver = new iCubDataReceiver(10/*1000.0 / MAIN_LOOP_FREQUENCY*/, iencs, this);
     data_receiver->start();
+}
 //! set the target position of a joint
 //! \param enum id of joint
 //! \param float value
 void iCubJointInterface::publish_target_position(int e){
     //first: convert humotion enum to our enum:
     int id = convert_enum_to_motorid(e);
     if (id == -1){
         return; //we are not interested in that data, so we just return here
+    }
     if (id == ICUB_ID_NECK_PAN){
         //PAN seems to be swapped
         store_joint(ICUB_ID_NECK_PAN, -joint_target[e]);
     }else if ((id == ICUB_ID_EYES_LEFT_LR) || ( id == ICUB_ID_EYES_RIGHT_LR)){
         //icub handles eyes differently, we have to set pan angle + vergence
         float pan      = (joint_target[ID_EYES_LEFT_LR] + joint_target[ID_EYES_RIGHT_LR]) / 2;
         float vergence = (joint_target[ID_EYES_LEFT_LR]  - joint_target[ID_EYES_RIGHT_LR]);
         //printf("EYEDBG %3.2f %3.2f --_> pan %3.2f verg=%3.2f\n",joint_target[ID_EYES_LEFT_LR], joint_target[ID_EYES_RIGHT_LR],pan,vergence);
         store_joint(ICUB_ID_EYES_PAN, pan);
         store_joint(ICUB_ID_EYES_VERGENCE, vergence);
     }else{
         store_joint(id, joint_target[e]);
+    }
+}
 //! set the target position of a joint
 //! \param id of joint
 //! \param float value of position
 void iCubJointInterface::store_joint(int id, float value){
     //printf("> set joint %d = %f\n",id,value);
     target_angle[id] = value;
     //ipos->positionMove(id, value);
+}
 //! execute a move in position mode
 //! \param id of joint
 //! \param angle
 void iCubJointInterface::set_target_in_positionmode(int id, double value){
     if (id>ICUB_ID_EYES_VERGENCE){
         printf("> ERROR: set_target_positionmode(id=%d, %3.2f) not supported for this id\n",id,value);
         return;
+    }
 #if 0
     //set speed cacluated as in velocity + set position -> replicates smoothmotion from flobi?!
     //first: calculate necessary speed to reach the given target within the next clock tick:
     double distance = value - target_angle_previous[id];
     //make the motion smooth: we want to reach 85% of the target in the next iteration:
     //calculate speed for that:
     double speed = 0.85 * distance * ((double)MAIN_LOOP_FREQUENCY);
     //set up speed for controller:
     ipos->setRefSpeed(id, speed);
 #endif
     //execute motion
     ipos->positionMove(id, value);
+}
 //! execute a move in velocity mode
 //! \param id of joint
 //! \param angle
 void iCubJointInterface::set_target_in_velocitymode(int id, double value){
     //first: calculate necessary speed to reach the given target within the next clock tick:
     double distance = value - target_angle_previous[id];
     //make the motion smooth: we want to reach 85% of the target in the next iteration:
     distance = 0.85 * distance;
     //calculate speed
     double speed = distance * ((double)MAIN_LOOP_FREQUENCY);
     //execute:
     ivel->velocityMove(id, speed);
     //if (id == ICUB_ID_NECK_PAN) printf("> VEL now=%3.2f target=%3.2f --> dist=%3.2f speed=%3.2f\n",target_angle_previous[id],value,distance,speed);
     target_angle_previous[id] = get_ts_position(convert_motorid_to_enum(id)).get_newest_value();
+}
 //! actually execute the scheduled motion commands
 void iCubJointInterface::execute_motion(){
     // set up neck and eye motion commands:
     if (POSITION_CONTROL){
         //position control
         for(int i=ICUB_ID_NECK_TILT; i<=ICUB_ID_EYES_VERGENCE; i++){
             set_target_in_positionmode(i, target_angle[i]);
+        }
     }else{
         //velocity control
         for(int i=ICUB_ID_NECK_TILT; i<=ICUB_ID_EYES_VERGENCE; i++){
             set_target_in_velocitymode(i, target_angle[i]);
+        }
+    }
     //printf("> TARGET PAN = %3.2f\n",target_angle[ICUB_ID_NECK_PAN]);
     //eyelids: unfortuantely the icub has only 1dof for eyelids, so we use only one dof here:
     set_eyelid_angle(target_angle[ICUB_ID_EYES_RIGHT_LID_UPPER]);
     //eyebrows are set using a special command as well:
     set_eyebrow_angle(ICUB_ID_EYES_LEFT_BROW);
     set_eyebrow_angle(ICUB_ID_EYES_RIGHT_BROW);
     //mouth
     set_mouth();
+}
 void iCubJointInterface::set_mouth(){
     //convert from 6DOF mouth displacement to icub leds:
     int led_value = 0;
     //fetch center opening:
     double center_opening = target_angle[ICUB_ID_LIP_CENTER_LOWER] - target_angle[ICUB_ID_LIP_CENTER_UPPER];
     bool mouth_open = (center_opening>15.0)?true:false;
     //side of mouth high or low?
     double center_avg = (target_angle[ICUB_ID_LIP_CENTER_LOWER] + target_angle[ICUB_ID_LIP_CENTER_UPPER])/2.0;
     double left_avg   = (target_angle[ICUB_ID_LIP_LEFT_LOWER] + target_angle[ICUB_ID_LIP_LEFT_UPPER])/2.0;
     double right_avg  = (target_angle[ICUB_ID_LIP_RIGHT_LOWER] + target_angle[ICUB_ID_LIP_RIGHT_UPPER])/2.0;
     //happy, neutral or sad?
     double diff_l = center_avg - left_avg;
     double diff_r = center_avg - right_avg;
     double diff   = (diff_l+diff_r)/2.0;
     if (diff > 2.0){
         if (mouth_open){
             led_value = 0x14;
         }else{
             if (diff > 2.6){
                 led_value = 0x0A;
             }else{
                 led_value = 0x0B;
+            }
+        }
     }else if (diff < -3.0){
         if (mouth_open){
             led_value = 0x06;
         }else{
             led_value = 0x18;
+        }
     }else if (diff < -2.0){
         if (mouth_open){
             led_value = 0x04; //0x25;
         }else{
             led_value = 0x08;
+        }
     }else{
         if (mouth_open){
             led_value = 0x16;
         }else{
             led_value = 0x08;
+        }
+    }
     if (led_value == previous_mouth_state){
         //no update necessary
         return;
+    }
     previous_mouth_state = led_value;
     //convert to string:
     char buf[10];
     sprintf(buf, "M%02X",led_value);
     /*printf("> sending mouth '%s'\n",buf);
     printf("> mouth angles: %3.2f %3.2f %3.2f\n",target_angle[ICUB_ID_LIP_LEFT_UPPER],target_angle[ICUB_ID_LIP_CENTER_UPPER],target_angle[ICUB_ID_LIP_RIGHT_UPPER]);
     printf("  mouth         %3.2f %3.2f %3.2f\n",target_angle[ICUB_ID_LIP_LEFT_LOWER],target_angle[ICUB_ID_LIP_CENTER_LOWER],target_angle[ICUB_ID_LIP_RIGHT_LOWER]);
     printf("  mouth  open=%3.2f diff=%3.2f\n", center_opening, diff);*/
     //add mouth:
     Bottle &cmd = emotion_port[3].prepare();
     cmd.clear();
     cmd.addString(buf);
     emotion_port[3].write();
     //store joint values which we do not handle on icub here:
     double timestamp = get_timestamp_ms();
     JointInterface::store_incoming_position(ID_LIP_LEFT_UPPER,   target_angle[ICUB_ID_LIP_LEFT_UPPER], timestamp);
     JointInterface::store_incoming_position(ID_LIP_LEFT_LOWER,   target_angle[ICUB_ID_LIP_LEFT_LOWER], timestamp);
     JointInterface::store_incoming_position(ID_LIP_CENTER_UPPER, target_angle[ICUB_ID_LIP_CENTER_UPPER], timestamp);
     JointInterface::store_incoming_position(ID_LIP_CENTER_LOWER, target_angle[ICUB_ID_LIP_CENTER_LOWER], timestamp);
     JointInterface::store_incoming_position(ID_LIP_RIGHT_UPPER,  target_angle[ICUB_ID_LIP_RIGHT_UPPER], timestamp);
     JointInterface::store_incoming_position(ID_LIP_RIGHT_LOWER,  target_angle[ICUB_ID_LIP_RIGHT_LOWER], timestamp);
+}
 double iCubJointInterface::get_timestamp_ms(){
     struct timespec spec;
     clock_gettime(CLOCK_REALTIME, &spec);
     return spec.tv_sec*1000 + spec.tv_nsec / 1.0e6;
+}
 //! set the current position of a joint
 //! \param id of joint
 //! \param float value of position
 //! \param double timestamp
 void iCubJointInterface::fetch_position(int id, double value, double timestamp){
     //store joint based on id:
     switch(id){
         default:
             printf("> ERROR: unhandled joint id %d\n",id);
             return;
         case(100):
             JointInterface::store_incoming_position(ID_EYES_RIGHT_LID_UPPER, lid_angle, timestamp);
             break;
         case(2):
             //PAN is inverted!
             JointInterface::store_incoming_position(ID_NECK_PAN, -value, timestamp);
             break;
         case(0):
             JointInterface::store_incoming_position(ID_NECK_TILT, value, timestamp);
             break;
         case(1):
             JointInterface::store_incoming_position(ID_NECK_ROLL, value, timestamp);
             break;
         case(3):
             JointInterface::store_incoming_position(ID_EYES_BOTH_UD, value, timestamp);
             break;
         //icub handles eyes differently, we have to set pan angle + vergence
         case(4): {//pan
             last_pos_eye_pan = value;
             float left  = last_pos_eye_pan + last_pos_eye_vergence/2.0;
             float right = last_pos_eye_pan - last_pos_eye_vergence/2.0;
             //printf("> eye: pan=%3.2f vergence=%3.2f --> L=%3.2f R=%3.2f\n", last_pos_eye_pan, last_pos_eye_vergence, left, right);
             JointInterface::store_incoming_position(ID_EYES_LEFT_LR, left, timestamp);
             JointInterface::store_incoming_position(ID_EYES_RIGHT_LR, right, timestamp);
             break;
+        }
         case(5): { //vergence
             last_pos_eye_vergence = value;
             float left  = last_pos_eye_pan + last_pos_eye_vergence/2.0;
             float right = last_pos_eye_pan - last_pos_eye_vergence/2.0;
             //printf("> eye: pan=%3.2f vergence=%3.2f --> L=%3.2f R=%3.2f\n", last_pos_eye_pan, last_pos_eye_vergence, left, right);
             JointInterface::store_incoming_position(ID_EYES_LEFT_LR, left, timestamp);
             JointInterface::store_incoming_position(ID_EYES_RIGHT_LR, right, timestamp);
             break;
+        }
+    }
+}
 //! set the current speed of a joint
 //! \param enum id of joint
 //! \param float value of speed
 //! \param double timestamp
 void iCubJointInterface::fetch_speed(int id, double value, double timestamp){
     switch(id){
         default:
             printf("> ERROR: unhandled joint id %d\n",id);
             return;
         case(2):
             JointInterface::store_incoming_speed(ID_NECK_PAN, value, timestamp);
             break;
         case(0):
             JointInterface::store_incoming_speed(ID_NECK_TILT, value, timestamp);
             break;
         case(1):
             JointInterface::store_incoming_speed(ID_NECK_ROLL, value, timestamp);
             break;
         case(3):
             JointInterface::store_incoming_speed(ID_EYES_BOTH_UD, value, timestamp);
             break;
         //icub handles eyes differently, we have to set pan angle + vergence
         case(4): {//pan
             last_vel_eye_pan = value;
             float left  = last_vel_eye_pan + last_vel_eye_vergence/2.0;
             float right = last_vel_eye_pan - last_vel_eye_vergence/2.0;
             //printf("> eye: velocity pan=%3.2f vergence=%3.2f --> L=%3.2f R=%3.2f\n", last_vel_eye_pan, last_vel_eye_vergence, left, right);
             JointInterface::store_incoming_speed(ID_EYES_LEFT_LR, left, timestamp);
             JointInterface::store_incoming_speed(ID_EYES_RIGHT_LR, right, timestamp);
             break;
+        }
         case(5): { //vergence
             last_vel_eye_pan = value;
             float left  = last_vel_eye_pan + last_vel_eye_vergence/2.0;
             float right = last_vel_eye_pan - last_vel_eye_vergence/2.0;
             //printf("> eye: velocity pan=%3.2f vergence=%3.2f --> L=%3.2f R=%3.2f\n", last_vel_eye_pan, last_vel_eye_vergence, left, right);
             JointInterface::store_incoming_speed(ID_EYES_LEFT_LR, left, timestamp);
             JointInterface::store_incoming_speed(ID_EYES_RIGHT_LR, right, timestamp);
             break;
+        }
+    }
 /*
     JointInterface::store_incoming_speed(ID_LIP_LEFT_UPPER, 0.0, timestamp);
     JointInterface::store_incoming_speed(ID_LIP_LEFT_LOWER, 0.0, timestamp);
     JointInterface::store_incoming_speed(ID_LIP_CENTER_UPPER, 0.0, timestamp);
     JointInterface::store_incoming_speed(ID_LIP_CENTER_LOWER, 0.0, timestamp);
     JointInterface::store_incoming_speed(ID_LIP_RIGHT_UPPER, 0.0, timestamp);
     JointInterface::store_incoming_speed(ID_LIP_RIGHT_LOWER, 0.0, timestamp);
     JointInterface::store_incoming_speed(ID_EYES_LEFT_LID_LOWER, 0.0, timestamp);
     JointInterface::store_incoming_speed(ID_EYES_LEFT_LID_UPPER, 0.0, timestamp);
     JointInterface::store_incoming_speed(ID_EYES_LEFT_BROW, 0.0, timestamp);
     JointInterface::store_incoming_speed(ID_EYES_RIGHT_LID_LOWER, 0.0, timestamp);
     JointInterface::store_incoming_speed(ID_EYES_RIGHT_LID_UPPER,0.0, timestamp);
     JointInterface::store_incoming_speed(ID_EYES_RIGHT_BROW, 0.0, timestamp);*/
     /*
     //fetch enum id:
     int e = convert_motorid_to_enum(device->get_device_id());
     if (e == -1){
         return; //we are not interested in that data, so we just return here
+    }
     JointInterface::store_incoming_speed(e, device->get_register(XSC3_REGISTER_MOTORSPEED), timestamp);*/
+}
 /*
 //! conversion table for humotion motor ids to our ids:
 //! \param enum from JointInterface::JOINT_ID_ENUM
 //! \return int value of motor id
 int HumotionJointInterface::convert_enum_to_motorid(int e){
     enum_id_bimap_t::right_const_iterator it = enum_id_bimap.right.find(e);
     if(it == enum_id_bimap.right.end()) {
         //key does not exists, we are not interested in that dataset, return -1
         return -1;
+    }
     return it->second;
+}
 //! conversion table for our ids to humotion motor ids:
 //! \param  int value of motor id
 //! \return enum from JointInterface::JOINT_ID_ENUM
 int HumotionJointInterface::convert_motorid_to_enum(int id){
     enum_id_bimap_t::left_const_iterator it = enum_id_bimap.left.find(id);
     if(it == enum_id_bimap.left.end()) {
         //key does not exists, we are not interested in that dataset, return -1
         return -1;
+    }
     return it->second;
+}
 */
 //! prepare and enable a joint
 //! NOTE: this should also prefill the min/max positions for this joint
 //! \param the enum id of a joint
 void iCubJointInterface::enable_joint(int e){
     //FIXME ADD THIS:
     // enable the amplifier and the pid controller on each joint
     /*for (i = 0; i < nj; i++) {
        amp->enableAmp(i);
        pid->enablePid(i);
     }*/
     //set up smooth motion controller
     //step1: set up framerate
     //dev->set_register_blocking(XSC3_REGISTER_PID_RAMP, framerate, true);
     //step2: set controllertype:
     //printf("> activating smooth motion control for joint id 0x%02X (%s)\n", motor_id, joint_name.c_str());
     //dev->set_register_blocking(XSC3_REGISTER_PID_CONTROLLER, XSC3_PROTOCOL_PID_CONTROLLERTYPE_SMOOTH_PLAYBACK, true);
     //step3: set pid controller:
     /*if ((e == ID_LIP_LEFT_UPPER) || (e == ID_LIP_LEFT_LOWER) || (e == ID_LIP_CENTER_UPPER) || (e == ID_LIP_CENTER_LOWER) || (e == ID_LIP_RIGHT_UPPER) || (e == ID_LIP_RIGHT_LOWER)){
         printf("> fixing PID i controller value for smooth motion (FIXME: restore old value!!)\n");
         dev->set_register_blocking(XSC3_REGISTER_CONST_I, 10, true);
     }*/
     //uint16_t result = dev->get_register_blocking_raw(XSC3_REGISTER_PID_CONTROLLER);
     //check if setting pid controllertype was successfull:
     /*if (result != XSC3_PROTOCOL_PID_CONTROLLERTYPE_SMOOTH_PLAYBACK){
         printf("> failed to set smooth motion controller for joint %s (res=0x%04X)\n",joint_name.c_str(),result);
         exit(1);
     }*/
     //fetch min/max:
    // init_joint(e);
     //ok fine, now enable motor:
     //printf("> enabling motor %s\n", joint_name.c_str());
     //dev->set_register_blocking(XSC3_REGISTER_STATUS, XSC3_PROTOCOL_STATUS_BITMASK_MOTOR_ENABLE, true);
+}
 void iCubJointInterface::store_min_max(IControlLimits *ilimits, int id, int e){
     double min, max;
     ilimits->getLimits(id, &min, &max);
     joint_min[e] = min;
     joint_max[e] = max;
+}
 //! initialise a joint (set up controller mode etc)
 //! \param joint enum
 void iCubJointInterface::init_joints(){
     store_min_max(ilimits, 0, ID_NECK_TILT);
     store_min_max(ilimits, 1, ID_NECK_ROLL);
     store_min_max(ilimits, 2, ID_NECK_PAN);
     store_min_max(ilimits, 3, ID_EYES_BOTH_UD);
     //icub handles eyes differently, we have to set pan angle + vergence
     double pan_min, pan_max, vergence_min, vergence_max;
     ilimits->getLimits(4, &pan_min, &pan_max);
     ilimits->getLimits(5, &vergence_min, &vergence_max);
     //this is not 100% correct, should be fixed:
     joint_min[ID_EYES_LEFT_LR] = pan_min; // - vergence_max/2;
     joint_max[ID_EYES_LEFT_LR] = pan_max; // - vergence_max/2;
     joint_min[ID_EYES_RIGHT_LR] = joint_min[ID_EYES_LEFT_LR];
     joint_max[ID_EYES_RIGHT_LR] = joint_max[ID_EYES_LEFT_LR];
     //eyelids:
     joint_min[ID_EYES_RIGHT_LID_UPPER] = -50; //24-30;
     joint_max[ID_EYES_RIGHT_LID_UPPER] = 50; //48-30;
     lid_angle = joint_max[ID_EYES_RIGHT_LID_UPPER];
     //eyebrows:
     joint_min[ID_EYES_LEFT_BROW] = -50;
     joint_max[ID_EYES_LEFT_BROW] = 50;
     joint_min[ID_EYES_RIGHT_BROW] = joint_min[ID_EYES_LEFT_BROW];
     joint_max[ID_EYES_RIGHT_BROW] = joint_max[ID_EYES_LEFT_BROW];
     //mouth:
     joint_min[ID_LIP_CENTER_UPPER] = 5;
     joint_max[ID_LIP_CENTER_UPPER] = 50;
     joint_min[ID_LIP_CENTER_LOWER] = 5;
     joint_max[ID_LIP_CENTER_LOWER] = 50;
     joint_min[ID_LIP_LEFT_UPPER] = 5;
     joint_max[ID_LIP_LEFT_UPPER] = 50;
     joint_min[ID_LIP_LEFT_LOWER] = 5;
     joint_max[ID_LIP_LEFT_LOWER] = 50;
     joint_min[ID_LIP_RIGHT_UPPER] = 5;
     joint_max[ID_LIP_RIGHT_UPPER] = 50;
     joint_min[ID_LIP_RIGHT_LOWER] = 5;
     joint_max[ID_LIP_RIGHT_LOWER] = 50;
+}
 //! shutdown and disable a joint
 //! \param the enum id of a joint
 void iCubJointInterface::disable_joint(int e){
     /*
         //first: convert humotion enum to our enum:
         int motor_id = convert_enum_to_motorid(e);
         if (motor_id == -1){
             return; //we are not interested in that data, so we just return here
+        }
         //fetch device:
         Device *dev = get_device(motor_id);
         printf("> FIXME: ADD DISABLE CODE\n");
         printf("> FIXME: ADD DISABLE CODE\n");
         printf("> FIXME: ADD DISABLE CODE\n");
         printf("> FIXME: ADD DISABLE CODE\n");
         printf("> FIXME: ADD DISABLE CODE\n");
         printf("> FIXME: ADD DISABLE CODE\n");
         printf("> FIXME: ADD DISABLE CODE\n");
         printf("> FIXME: ADD DISABLE CODE\n");
         printf("> FIXME: ADD DISABLE CODE\n");
         */
+}
-a6a6c
+#endif