commit 2be6243ff899e54125f58ea35feebabb21bc2d1c
Author: Sebastian Meyer zu Borgsen <semeyerz@techfak.uni-bielefeld.de>
Date:   Fri Nov 27 13:51:45 2015 +0100

    adding meka controller

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 59655b0..d86f305 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -8,11 +8,12 @@ set(ROS_BUILD_TYPE Debug)
 
 
 #######################################
-# check if we have RSB support:
-
-option(WITH_RSB "WITH_RSB" ON)
+#allow forced disable of RSB
+option(IGNORE_RSB "IGNORE_RSB" OFF)
 
-IF(WITH_RSB)
+#######################################
+# check if we have RSB support:
+IF (not IGNORE_RSB)
 FIND_PACKAGE(RSB 0.11)
 IF (RSB_FOUND)
     #RSB
@@ -41,7 +42,8 @@ IF (RSB_FOUND)
     message(STATUS "RSB Support is ON")
     add_definitions(-DRSB_SUPPORT=1)
 ENDIF (RSB_FOUND)
-ENDIF(WITH_RSB)
+ENDIF(IGNORE_RSB)
+
 ################################################################
 # check for ROS support:
 find_package(catkin REQUIRED COMPONENTS roscpp std_msgs sensor_msgs message_generation genmsg)
diff --git a/examples/meka/CMakeLists.txt b/examples/meka/CMakeLists.txt
new file mode 100644
index 0000000..eb4584c
--- /dev/null
+++ b/examples/meka/CMakeLists.txt
@@ -0,0 +1,26 @@
+PROJECT(icub_humotion)
+cmake_minimum_required(VERSION 2.8)
+
+FIND_PACKAGE(Boost REQUIRED COMPONENTS system thread)
+FIND_PACKAGE(humotion)
+
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
+
+# add include directories
+INCLUDE_DIRECTORIES(${YARP_INCLUDE_DIRS} ${ICUB_INCLUDE_DIRS} ${humotion_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} include/)
+link_directories(${Boost_LIBRARY_DIRS} ${humotion_LIBRARY_DIRS})
+
+# add required linker flags
+SET(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${ICUB_LINK_FLAGS}")
+SET(MAIN icub_humotion_server)
+
+file(GLOB DUMMY_HEADER_LIST RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} include/*.h)
+
+ADD_EXECUTABLE(${MAIN} src/main.cpp src/mekajointinterface.cpp src/meka_data_receiver.cpp ${DUMMY_HEADER_LIST})
+
+TARGET_LINK_LIBRARIES(${MAIN} ${Boost_LIBRARIES} ${humotion_LIBRARIES})
+set_property(TARGET ${MAIN} PROPERTY INSTALL_RPATH_USE_LINK_PATH TRUE)
+
+
+INSTALL(TARGETS ${MAIN} DESTINATION bin)
+
diff --git a/examples/meka/include/mekajointinterface.h b/examples/meka/include/mekajointinterface.h
new file mode 100644
index 0000000..59a745d
--- /dev/null
+++ b/examples/meka/include/mekajointinterface.h
@@ -0,0 +1,86 @@
+#pragma once
+#include <humotion/server/joint_interface.h>
+#include <humotion/server/server.h>
+#include <boost/bimap.hpp>
+//#include "meka_data_receiver.h"
+//class MekaDataReceiver;
+
+class MekaJointInterface : public humotion::server::JointInterface{
+public:
+    MekaJointInterface(std::string scope);
+    ~MekaJointInterface();
+
+    //void fetch_position(Device *dev, double timestamp);
+    //void fetch_speed(Device *dev, double timestamp);
+    void fetch_position(int id, double value, double timestamp);
+    void fetch_speed(int id, double value, double timestamp);
+    void run();
+
+    enum JOINT_ID_ENUM{
+        ICUB_ID_NECK_TILT = 0,
+        ICUB_ID_NECK_ROLL = 1,
+        ICUB_ID_NECK_PAN  = 2,
+        ICUB_ID_EYES_BOTH_UD = 3,
+        ICUB_ID_EYES_PAN = 4,
+        ICUB_ID_EYES_VERGENCE = 5,
+        //the following ids are used for remapping:
+        ICUB_ID_EYES_LEFT_LR,
+        ICUB_ID_EYES_RIGHT_LR,
+        ICUB_ID_EYES_LEFT_LID_LOWER,
+        ICUB_ID_EYES_LEFT_LID_UPPER,
+        ICUB_ID_EYES_RIGHT_LID_LOWER,
+        ICUB_ID_EYES_RIGHT_LID_UPPER,
+        ICUB_ID_EYES_LEFT_BROW,
+        ICUB_ID_EYES_RIGHT_BROW,
+        ICUB_ID_LIP_LEFT_UPPER,
+        ICUB_ID_LIP_LEFT_LOWER,
+        ICUB_ID_LIP_CENTER_UPPER,
+        ICUB_ID_LIP_CENTER_LOWER,
+        ICUB_ID_LIP_RIGHT_UPPER,
+        ICUB_ID_LIP_RIGHT_LOWER,
+        ICUB_JOINT_ID_ENUM_SIZE
+    };
+
+    static const int MAIN_LOOP_FREQUENCY = 50;
+
+protected:
+    void disable_joint(int e);
+    void publish_target_position(int e);
+    void enable_joint(int e);
+    void execute_motion();
+
+private:
+    double get_timestamp_ms();
+    double target_angle[ICUB_JOINT_ID_ENUM_SIZE];
+    double target_angle_previous[ICUB_JOINT_ID_ENUM_SIZE];
+
+    void set_eyelid_angle(double angle);
+    void set_eyebrow_angle(int id);
+    void set_mouth();
+
+    //iCubDataReceiver *icub_data_receiver;
+    void init_joints();
+    double lid_angle;
+    int lid_opening_previous;
+    int previous_mouth_state;
+
+    std::string scope;
+
+    float last_pos_eye_vergence;
+    float last_pos_eye_pan;
+    float last_vel_eye_vergence;
+    float last_vel_eye_pan;
+
+    void store_joint(int id, float value);
+    void set_target_in_positionmode(int id, double value);
+    void set_target_in_velocitymode(int id, double value);
+
+
+    int convert_enum_to_motorid(int e);
+    int convert_motorid_to_enum(int id);
+
+
+    typedef boost::bimap<int, int > enum_id_bimap_t;
+    typedef enum_id_bimap_t::value_type enum_id_bimap_entry_t;
+    enum_id_bimap_t enum_id_bimap;
+};
diff --git a/examples/meka/src/main.cpp b/examples/meka/src/main.cpp
new file mode 100644
index 0000000..405421c
--- /dev/null
+++ b/examples/meka/src/main.cpp
@@ -0,0 +1,38 @@
+#include <stdio.h>
+#include <humotion/server/server.h>
+#include <string>
+#include <iostream>
+//#include "meka_data_receiver.h"
+#include "mekajointinterface.h"
+
+using namespace std;
+
+int main(int argc, char *argv[]){
+    /*Property params;
+    params.fromCommand(argc, argv);
+
+    if (!params.check("robot")){
+        fprintf(stderr, "Please specify the name of the robot\n");
+        fprintf(stderr, "--robot name (e.g. icub or icubSim)\n");
+        return -1;
+    }
+
+    string robotName=params.find("robot").asString().c_str();
+    string scope="/"+robotName;
+
+*/
+
+    //create humotion interface
+    string scope = "/bla";
+    MekaJointInterface *jointinterface = new MekaJointInterface(scope);
+    humotion::server::Server *humotion_server = new humotion::server::Server("/meka", "ROS", jointinterface);
+
+    //finallz run it
+    jointinterface->run();
+
+    while(humotion_server->ok()){
+        usleep(100000);
+    }
+
+    return 0;
+}
diff --git a/examples/meka/src/mekajointinterface.cpp b/examples/meka/src/mekajointinterface.cpp
new file mode 100644
index 0000000..36a79b0
--- /dev/null
+++ b/examples/meka/src/mekajointinterface.cpp
@@ -0,0 +1,706 @@
+#include "mekajointinterface.h"
+using namespace std;
+
+//WARNING: DO NOT CHANGE THIS; VELOCITYMODE IS NOT YET IMPLEMENTED
+#define POSITION_CONTROL 1
+
+//! constructor
+MekaJointInterface::MekaJointInterface(string _scope) : humotion::server::JointInterface(){
+    scope = _scope;
+
+    //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(){
+}
+
+/*
+
+//! 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");
+        */
+}
