commit 35b3ca25ef9f69054575a5e8fc699b38a3aeeea2
Author: Simon Schulz <sschulz@cit-ec.uni-bielefeld.de>
Date:   Wed Feb 10 14:57:37 2016 +0100

    restructured and cleaned up, breaks xsc3

diff --git a/examples/yarp_icub/include/icub_data_receiver.h b/examples/yarp_icub/include/icub_data_receiver.h
index 3d3cd10..8baaddd 100644
--- a/examples/yarp_icub/include/icub_data_receiver.h
+++ b/examples/yarp_icub/include/icub_data_receiver.h
@@ -20,19 +20,21 @@ class iCubJointInterface;
 
 class iCubDataReceiver : public yarp::os::RateThread{
 public:
-    iCubDataReceiver(int period, yarp::dev::IEncodersTimed *_iencs, iCubJointInterface *_icub_jointinterface);
+    iCubDataReceiver(int period, iCubJointInterface *icub_jointinterface);
     bool threadInit();
     void threadRelease();
     void run();
 private:
-    double get_timestamp_ms();
+    void store_incoming_position(int icub_id, double value, double timestamp);
 
-    yarp::sig::Vector positions;
-    yarp::sig::Vector velocities;
-    yarp::sig::Vector commands;
-    yarp::sig::Vector timestamps;
+    float target_eye_pan_;
+    float target_eye_vergence_;
 
-    yarp::dev::IEncodersTimed *iencs;
-    iCubJointInterface *icub_jointinterface;
+    yarp::sig::Vector yarp_positions_;
+    yarp::sig::Vector yarp_commands_;
+    yarp::sig::Vector yarp_timestamps_;
+
+    iCubJointInterface *icub_jointinterface_;
+    yarp::dev::IEncodersTimed *yarp_iencs_;
 };
 
diff --git a/examples/yarp_icub/include/icub_faceinterface.h b/examples/yarp_icub/include/icub_faceinterface.h
index edcf695..dd27d53 100644
--- a/examples/yarp_icub/include/icub_faceinterface.h
+++ b/examples/yarp_icub/include/icub_faceinterface.h
@@ -19,9 +19,9 @@ public:
     ~iCubFaceInterface();
 
 
-    void set_eyelid_angle(double angle);
-    void set_eyebrow_angle(int id, double *target_angle);
-    void set_mouth(double *target_angle);
+    void set_eyelid_angle(float angle);
+    void set_eyebrow_angle(int id, float *target_angle);
+    void set_mouth(float *target_angle);
 
 private:
     double lid_angle;
diff --git a/examples/yarp_icub/include/icub_jointinterface.h b/examples/yarp_icub/include/icub_jointinterface.h
index 1a574e2..9cdd232 100644
--- a/examples/yarp_icub/include/icub_jointinterface.h
+++ b/examples/yarp_icub/include/icub_jointinterface.h
@@ -25,12 +25,12 @@ public:
     iCubJointInterface(std::string scope);
     ~iCubJointInterface();
 
-    //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 store_incoming_position(int id, double value, double timestamp);
     void run();
 
+    int convert_icub_jointid_to_humotion(int id);
+    int convert_humotion_jointid_to_icub(int id);
+
     enum JOINT_ID_ENUM{
         ICUB_ID_NECK_TILT = 0,
         ICUB_ID_NECK_ROLL = 1,
@@ -38,9 +38,6 @@ public:
         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,
@@ -56,39 +53,60 @@ public:
         ICUB_JOINT_ID_ENUM_SIZE
     };
 
+    yarp::dev::PolyDriver *get_yarp_polydriver() { return &yarp_polydriver_; }
+
     static const int MAIN_LOOP_FREQUENCY = 50;
 
 protected:
     void disable_joint(int e);
-    void publish_target_position(int e);
+    void publish_target(int e, float position, float velocity);
     void enable_joint(int e);
     void execute_motion();
 
 private:
+    yarp::dev::PolyDriver yarp_polydriver_;
+
+    std::vector<double> pv_mix_last_error_;
+    std::vector<double> pv_mix_pid_p_;
+    std::vector<double> pv_mix_pid_d_;
+
+    // yarp views
+    yarp::dev::IVelocityControl *yarp_ivel_;
+    yarp::dev::IPositionControl *yarp_ipos_;
+    //yarp::dev::IEncodersTimed *yarp_iencs_;
+    yarp::dev::IControlLimits *yarp_ilimits_;
+    yarp::dev::IAmplifierControl *yarp_amp_;
+    yarp::dev::IPidControl *yarp_pid_;
+    yarp::dev::IControlMode *yarp_icontrol_;
+
+    yarp::sig::Vector yarp_commands_;
+
+    float target_angle_[ICUB_JOINT_ID_ENUM_SIZE];
+    float target_velocity_[ICUB_JOINT_ID_ENUM_SIZE];
+
+
+    void init_controller();
+    void init_id_map();
+    void init_pv_mix_pid();
+    void insert_icupid_to_humotionid_mapping(int icubid, int humotionid);
+
+
+    void store_icub_joint_target(int icub_id, float position, float velocity);
+
+    //*******************************************
+
+
     void set_joint_enable_state(int e, bool enabled);
-    double get_timestamp_ms();
-    double target_angle[ICUB_JOINT_ID_ENUM_SIZE];
-    double target_angle_previous[ICUB_JOINT_ID_ENUM_SIZE];
+    //double target_angle[ICUB_JOINT_ID_ENUM_SIZE];
+    //double target_angle_previous[ICUB_JOINT_ID_ENUM_SIZE];
 
     iCubDataReceiver *icub_data_receiver;
     void init_joints();
 
     std::string scope;
-    yarp::dev::PolyDriver dd;
     yarp::sig::Vector positions;
     yarp::sig::Vector velocities;
-    yarp::sig::Vector commands;
-
-    yarp::dev::IVelocityControl *ivel;
-    yarp::dev::IPositionControl *ipos;
-    yarp::dev::IEncodersTimed *iencs;
-    yarp::dev::IControlLimits *ilimits;
-    yarp::dev::IAmplifierControl *amp;
-    yarp::dev::IPidControl *pid;
-    yarp::dev::IControlMode *icontrol;
-    std::vector<double> last_position_error;
-    std::vector<double> PID_P;
-    std::vector<double> PID_D;
+
 
     void store_min_max(yarp::dev::IControlLimits *ilimits, int id, int e);
 
@@ -105,7 +123,7 @@ private:
     int convert_enum_to_motorid(int e);
     int convert_motorid_to_enum(int id);
 
-    iCubFaceInterface *face_interface;
+    iCubFaceInterface *face_interface_;
 
     typedef boost::bimap<int, int > enum_id_bimap_t;
     typedef enum_id_bimap_t::value_type enum_id_bimap_entry_t;
diff --git a/examples/yarp_icub/src/icub_data_receiver.cpp b/examples/yarp_icub/src/icub_data_receiver.cpp
index f01302d..a858e33 100644
--- a/examples/yarp_icub/src/icub_data_receiver.cpp
+++ b/examples/yarp_icub/src/icub_data_receiver.cpp
@@ -1,50 +1,86 @@
 #include "icub_data_receiver.h"
+#include <humotion/server/joint_interface.h>
 #include <yarp/os/Property.h>
-using namespace yarp::dev;
-using namespace yarp::sig;
-using namespace yarp::os;
-using namespace std;
-
-iCubDataReceiver::iCubDataReceiver(int period, IEncodersTimed *_iencs, iCubJointInterface *_icub_jointinterface):RateThread(period){
-    iencs = _iencs;
-    icub_jointinterface = _icub_jointinterface;
+//using namespace yarp::dev;
+//using namespace yarp::sig;
+//using namespace yarp::os;
+using std::cout;
+using std::string;
+
+using humotion::server::JointInterface;
+using yarp::dev::IEncodersTimed;
+using yarp::sig::Vector;
+
+iCubDataReceiver::iCubDataReceiver(int period, iCubJointInterface *icub_jointinterface)
+    : yarp::os::RateThread(period) {
+
+    // store pointer to icub jointinterface
+    icub_jointinterface_ = icub_jointinterface;
+
+    //fetch yarp iencs view:
+    yarp::dev::PolyDriver *poly_driver = icub_jointinterface->get_yarp_polydriver();
+    bool success = poly_driver->view(yarp_iencs_);
+    if (!success) {
+        cout << "ERROR: polydriver failed to init iencs view\n";
+        exit(EXIT_FAILURE);
+    }
+
+    // resize data storage vectors to match the number of axes:
     int joints;
-    iencs->getAxes(&joints);
-    positions.resize(joints);
-    velocities.resize(joints);
-    timestamps.resize(joints);
+    yarp_iencs_->getAxes(&joints);
+    yarp_positions_.resize(joints);
+    yarp_timestamps_.resize(joints);
 }
 
-bool iCubDataReceiver::threadInit(){ return true; }
-void iCubDataReceiver::threadRelease(){ }
-
-double iCubDataReceiver::get_timestamp_ms(){
-    struct timespec spec;
-    clock_gettime(CLOCK_REALTIME, &spec);
-    return spec.tv_sec+spec.tv_nsec/1000000000.0;
+bool iCubDataReceiver::threadInit() {
+    return true;
 }
 
-void iCubDataReceiver::run(){
+void iCubDataReceiver::threadRelease() {
+}
 
+void iCubDataReceiver::run() {
     //grab pos+vel data:
-    iencs->getEncodersTimed(positions.data(), timestamps.data());
-    iencs->getEncoderSpeeds(velocities.data());
-
-    //double timestamp = get_timestamp_ms();
+    yarp_iencs_->getEncodersTimed(yarp_positions_.data(), yarp_timestamps_.data());
+    //iencs->getEncoderSpeeds(velocities.data());
 
     //publish data to humotion
-    for(int i=0; i<positions.size(); i++){
-        icub_jointinterface->fetch_position(i, positions[i], timestamps[i]);
-        icub_jointinterface->fetch_speed(i, velocities[i], timestamps[i]);
-        if (i==iCubJointInterface::ICUB_ID_NECK_PAN){
-            printf("\nMMM p=%f v=%f\n",positions[i],velocities[i]);
-        }
+    for(int i=0; i<yarp_positions_.size(); i++){
+        store_incoming_position(i, yarp_positions_[i], yarp_timestamps_[i]);
     }
 
-    //printf("\n%f %f %f TIME\n", get_timestamp_ms(), timestamps[2], positions[2]);
-    //printf("TIMEDIFF %f\n", get_timestamp_ms() - timestamps[2]);
-
-    //tell humotion to update lid angle (hack)
-    icub_jointinterface->fetch_position(100, 0.0, get_timestamp_ms());
+    //small hack to tell humotion to update the lid angle
     //fixme: use real id
+    store_incoming_position(100, 0.0, yarp_timestamps_[0]);
+}
+
+void iCubDataReceiver::store_incoming_position(int icub_id, double value, double timestamp) {
+    // store joint position in humotion backend
+    if ((icub_id == iCubJointInterface::ICUB_ID_EYES_PAN) ||
+            (icub_id == iCubJointInterface::ICUB_ID_EYES_VERGENCE)) {
+        // the icub handles eyes differently
+        // instead of using seperate left/right pan the icub uses
+        // a combined pan angle and vergence. therfore we have to convert this here:
+        if (icub_id == iCubJointInterface::ICUB_ID_EYES_PAN) {
+            target_eye_pan_ = value;
+        } else {
+            target_eye_vergence_ = value;
+        }
+
+        float left  = target_eye_pan_ + target_eye_vergence_/2.0;
+        float right = target_eye_pan_ - target_eye_vergence_/2.0;
+
+        icub_jointinterface_->store_incoming_position(JointInterface::ID_EYES_LEFT_LR,
+                                                     left, timestamp);
+        icub_jointinterface_->store_incoming_position(JointInterface::ID_EYES_RIGHT_LR,
+                                                     right, timestamp);
+    } else if (icub_id == 100) {
+        //HACK
+        //icub_jointinterface->store_incoming_position(ID_EYES_RIGHT_LID_UPPER,
+        //                                             lid_angle, timestamp);
+    } else {
+        // known configured mapping between joint ids
+        int humotion_id = icub_jointinterface_->convert_icub_jointid_to_humotion(icub_id);
+        icub_jointinterface_->store_incoming_position(humotion_id, value, timestamp);
+    }
 }
diff --git a/examples/yarp_icub/src/icub_faceinterface.cpp b/examples/yarp_icub/src/icub_faceinterface.cpp
index 9b2e6ee..2a28117 100644
--- a/examples/yarp_icub/src/icub_faceinterface.cpp
+++ b/examples/yarp_icub/src/icub_faceinterface.cpp
@@ -108,7 +108,7 @@ iCubFaceInterface::~iCubFaceInterface(){
 
 //! special command to set eyelid angle
 //! \param angle in degrees
-void iCubFaceInterface::set_eyelid_angle(double angle){
+void iCubFaceInterface::set_eyelid_angle(float 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°
@@ -140,7 +140,7 @@ void iCubFaceInterface::set_eyelid_angle(double angle){
 //! special command to set the eyebrow angle
 //! \param id {0=left, 1=right)
 //! \param angle in degrees
-void iCubFaceInterface::set_eyebrow_angle(int id, double *target_angle){
+void iCubFaceInterface::set_eyebrow_angle(int id, float *target_angle){
     int port_id;
     if (id == iCubJointInterface::ICUB_ID_EYES_LEFT_BROW){
         port_id = 1;
@@ -195,7 +195,7 @@ void iCubFaceInterface::set_eyebrow_angle(int id, double *target_angle){
     }
 }
 
-void iCubFaceInterface::set_mouth(double *target_angle){
+void iCubFaceInterface::set_mouth(float *target_angle){
     //convert from 6DOF mouth displacement to icub leds:
     int led_value = 0;
 
diff --git a/examples/yarp_icub/src/icub_jointinterface.cpp b/examples/yarp_icub/src/icub_jointinterface.cpp
index e3e1867..1f08dba 100644
--- a/examples/yarp_icub/src/icub_jointinterface.cpp
+++ b/examples/yarp_icub/src/icub_jointinterface.cpp
@@ -17,257 +17,258 @@ TEST: yarp write /writer /icubSim/face/raw/in
 http://wiki.icub.org/wiki/Motor_control
 */
 
-//WARNING: DO NOT CHANGE THIS; VELOCITYMODE IS NOT YET IMPLEMENTED
 #define POSITION_CONTROL 0
-
+using std::cout;
 
 //! constructor
 iCubJointInterface::iCubJointInterface(string _scope) : humotion::server::JointInterface(){
     scope = _scope;
-    face_interface = new iCubFaceInterface(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_PAN,   ID_EYES_LEFT_LR));
-    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_VERGENCE,   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));
-
-    //init pd control variables
-    enum_id_bimap_t::const_iterator it;
-    last_position_error.resize(ICUB_JOINT_ID_ENUM_SIZE);
-    PID_P.resize(ICUB_JOINT_ID_ENUM_SIZE);
-    PID_D.resize(ICUB_JOINT_ID_ENUM_SIZE);
 
-    for(it = enum_id_bimap.begin(); it != enum_id_bimap.end(); ++it) {
-        int id = it->left;
-        last_position_error[id] = 0.0;
-        PID_P[id] = 4.5;
-        PID_D[id] = 0.3;
-    }
+    // add mappings from icub ids to humotion ids
+    init_id_map();
+
+    // initialise the pd controller for the velocity and position mixer
+    init_pv_mix_pid();
 
+    // intantiate the face interface
+    face_interface_ = new iCubFaceInterface(scope);
 
+    // intantiate the polydriver
     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);
-    dd.view(pid);
-    dd.view(amp);
-
-
-    if ( (!iencs) || (!ipos) || (!ilimits) || (!ivel) || (!amp) || (!pid)){
-        printf("> ERROR: failed to open icub views\n");
+    options.put("remote", scope + "/head");
+    yarp_polydriver_.open(options);
+
+    // fetch yarp views:
+    bool success = true;
+    //success &= yarp_polydriver_.view(yarp_iencs_);
+    success &= yarp_polydriver_.view(yarp_ipos_);
+    success &= yarp_polydriver_.view(yarp_ivel_);
+    success &= yarp_polydriver_.view(yarp_ilimits_);
+    success &= yarp_polydriver_.view(yarp_pid_);
+    success &= yarp_polydriver_.view(yarp_amp_);
+
+    if (!success) {
+        cout << "ERROR: failed to fetch one or more yarp views... exiting\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);
+    //initialise joint controller
+    init_controller();
+}
+
+//! destructor
+iCubJointInterface::~iCubJointInterface(){
+}
+
+//! init the controller that allows to write target angles or velocities
+void iCubJointInterface::init_controller() {
+    int number_of_joints;
 
     //set position mode:
     if (POSITION_CONTROL){
-        commands=200000.0;
-        ipos->setRefAccelerations(commands.data());
-        ipos->setPositionMode();
+        // use position controller, first fetch no of axes:
+        yarp_ipos_->getAxes(&number_of_joints);
+
+        // set ref acceleration to a value for all axes:
+        yarp_commands_.resize(number_of_joints);
+        yarp_commands_ = 200000.0;
+        yarp_ipos_->setRefAccelerations(yarp_commands_.data());
+        yarp_ipos_->setPositionMode();
     }else{
-        ivel->setVelocityMode();
-        commands=300.0;
-        ivel->setRefAccelerations(commands.data());
+        // use position controller, first fetch no of axes:
+        yarp_ivel_->getAxes(&number_of_joints);
+
+        // set ref acceleration to a value for all axes:
+        yarp_commands_.resize(number_of_joints);
+        yarp_commands_=300.0;
+        yarp_ivel_->setRefAccelerations(yarp_commands_.data());
+        yarp_ivel_->setVelocityMode();
     }
 
-}
 
-//! destructor
-iCubJointInterface::~iCubJointInterface(){
 }
 
+//! initialise icub joint id to humotion joint id mappings
+void iCubJointInterface::init_id_map() {
+        insert_icupid_to_humotionid_mapping(ICUB_ID_LIP_LEFT_UPPER,   ID_LIP_LEFT_UPPER);
+        insert_icupid_to_humotionid_mapping(ICUB_ID_LIP_LEFT_LOWER,   ID_LIP_LEFT_LOWER);
+        insert_icupid_to_humotionid_mapping(ICUB_ID_LIP_CENTER_UPPER, ID_LIP_CENTER_UPPER);
+        insert_icupid_to_humotionid_mapping(ICUB_ID_LIP_CENTER_LOWER, ID_LIP_CENTER_LOWER);
+        insert_icupid_to_humotionid_mapping(ICUB_ID_LIP_RIGHT_UPPER,  ID_LIP_RIGHT_UPPER);
+        insert_icupid_to_humotionid_mapping(ICUB_ID_LIP_RIGHT_LOWER,  ID_LIP_RIGHT_LOWER);
+        insert_icupid_to_humotionid_mapping(ICUB_ID_NECK_PAN,    ID_NECK_PAN);
+        insert_icupid_to_humotionid_mapping(ICUB_ID_NECK_TILT,   ID_NECK_TILT);
+        insert_icupid_to_humotionid_mapping(ICUB_ID_NECK_ROLL,   ID_NECK_ROLL);
+        // FIXME: remove this hack tha repurposes LEFT/RIGHT eye pan from humotion as vergence/pan
+        insert_icupid_to_humotionid_mapping(ICUB_ID_EYES_PAN,   ID_EYES_LEFT_LR);
+        insert_icupid_to_humotionid_mapping(ICUB_ID_EYES_VERGENCE,   ID_EYES_RIGHT_LR);
+        insert_icupid_to_humotionid_mapping(ICUB_ID_EYES_BOTH_UD,   ID_EYES_BOTH_UD);
+        insert_icupid_to_humotionid_mapping(ICUB_ID_EYES_LEFT_LID_LOWER, ID_EYES_LEFT_LID_LOWER);
+        insert_icupid_to_humotionid_mapping(ICUB_ID_EYES_LEFT_LID_UPPER, ID_EYES_LEFT_LID_UPPER);
+        insert_icupid_to_humotionid_mapping(ICUB_ID_EYES_LEFT_BROW, ID_EYES_LEFT_BROW);
+        insert_icupid_to_humotionid_mapping(ICUB_ID_EYES_RIGHT_LID_LOWER, ID_EYES_RIGHT_LID_LOWER);
+        insert_icupid_to_humotionid_mapping(ICUB_ID_EYES_RIGHT_LID_UPPER,ID_EYES_RIGHT_LID_UPPER);
+        insert_icupid_to_humotionid_mapping(ICUB_ID_EYES_RIGHT_BROW, ID_EYES_RIGHT_BROW);
+}
 
+//! initialize the position and velocity mixer PD controller
+void iCubJointInterface::init_pv_mix_pid() {
+    // init control variables and last error variable for the internal
+    // position and velocity mixer PD controller:
+    pv_mix_pid_p_.resize(ICUB_JOINT_ID_ENUM_SIZE);
+    pv_mix_pid_d_.resize(ICUB_JOINT_ID_ENUM_SIZE);
+    pv_mix_last_error_.resize(ICUB_JOINT_ID_ENUM_SIZE);
 
-//! 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;
+    enum_id_bimap_t::const_iterator it;
+    for(it = enum_id_bimap.begin(); it != enum_id_bimap.end(); ++it) {
+        int id = it->left;
+        pv_mix_pid_p_[id] = 4.5;
+        pv_mix_pid_d_[id] = 0.3;
+        pv_mix_last_error_[id] = 0.0;
     }
-    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;
+//! add mapping from icub joint ids to humotion ids
+//! 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 the icub motor ids.
+void iCubJointInterface::insert_icupid_to_humotionid_mapping(int icubid, int humotionid) {
+    enum_id_bimap.insert(enum_id_bimap_entry_t(icubid, humotionid));
 }
 
 
+
+
 void iCubJointInterface::run(){
-    iCubDataReceiver *data_receiver = new iCubDataReceiver(0.5 * 1000.0 / MAIN_LOOP_FREQUENCY, iencs, this);
+    float loop_duration_ms = 1000.0 / MAIN_LOOP_FREQUENCY;
+    iCubDataReceiver *data_receiver = new iCubDataReceiver(loop_duration_ms, this);
     data_receiver->start();
 }
 
-//! set the target position of a joint
+//! stores the target position & velocity of a given 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_PAN) || ( id == ICUB_ID_EYES_VERGENCE)){
-        //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);
+void iCubJointInterface::publish_target(int humotion_id, float position, float velocity){
+    // special handler for eye joints
+    if ((humotion_id == JointInterface::ID_EYES_LEFT_LR) ||
+        (humotion_id == JointInterface::ID_EYES_RIGHT_LR)){
+        // the icub has a combined pan angle for both eyes, so seperate this:
+        float target_position_left  = get_target_position(JointInterface::ID_EYES_LEFT_LR);
+        float target_position_right = get_target_position(JointInterface::ID_EYES_RIGHT_LR);
+        float target_velocity_left  = get_target_velocity(JointInterface::ID_EYES_LEFT_LR);
+        float target_velocity_right = get_target_velocity(JointInterface::ID_EYES_RIGHT_LR);
+
+        // calculate target angles
+        float target_position_pan      = (target_position_left + target_position_right) / 2;
+        float target_position_vergence = (target_position_left - target_position_right);
+
+        // calculate target velocities
+        // for now just use the same velocity for pan and vergence
+        float target_velocity_pan = (target_velocity_left + target_velocity_right) / 2.0;
+        float target_velocity_tilt = target_velocity_pan;
+
+        store_icub_joint_target(ICUB_ID_EYES_PAN,
+                                target_position_pan, target_velocity_pan);
+        store_icub_joint_target(ICUB_ID_EYES_VERGENCE,
+                                target_position_vergence, target_velocity_tilt);
     }else{
-        store_joint(id, joint_target[e]);
+        // convert to icub joint id
+        int icub_id = convert_humotion_jointid_to_icub(humotion_id);
+        // store target data
+        store_icub_joint_target(icub_id, position, velocity);
     }
 }
 
 
-//! set the target position of a joint
+//! set the target data for a given icub 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;
+void iCubJointInterface::store_icub_joint_target(int icub_id, float position, float velocity) {
+    printf("> set icub joint %d -> p = %f, v = %f\n",icub_id,position,velocity);
+    target_angle_[icub_id] = position;
+    target_velocity_[icub_id] = velocity;
 }
 
 //! execute a move in position mode
 //! \param id of joint
 //! \param angle
-void iCubJointInterface::set_target_in_positionmode(int id){
-    double target = target_angle[id];
+void iCubJointInterface::set_target_in_positionmode(int id) {
+    assert(POSITION_CONTROL);
+    assert(id<=ICUB_ID_EYES_VERGENCE);
 
-    if (id>ICUB_ID_EYES_VERGENCE){
-        printf("> ERROR: set_target_positionmode(id=%d, %3.2f) not supported for this id\n",id,target);
-        return;
-    }
+    // fetch the current target position
+    float target = target_angle_[id];
 
     // execute motion as position control cmd
-    ipos->positionMove(id, target);
-
+    yarp_ipos_->positionMove(id, target);
 }
 
 //! execute a move in velocity mode
 //! \param id of joint
 //! \param angle
-void iCubJointInterface::set_target_in_velocitymode(int id){
-    // 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 = target_angle[id] - target_angle_previous[id];
-
-    //make the motion smooth: we want to reach 85% of the target in the next iteration:
-    distance = 0.85 * distance;
-
-    //distance = -5.0 / 50.0;
-
-    //calculate speed
-    //double speed = distance * ((double)MAIN_LOOP_FREQUENCY);
+void iCubJointInterface::set_target_in_velocitymode(int icub_id) {
+    assert(!POSITION_CONTROL);
 
+    // fetch humotion id from icub joint id
+    int humotion_id = convert_humotion_jointid_to_icub(icub_id);
 
+    // fetch the target velocity
+    float target_velocity = target_velocity_[icub_id];
 
-    int e = convert_motorid_to_enum(id);
-    double speed = joint_target_speed[e];
-
-    double max = 150.0;
-    if (speed > max) speed = max;
-    if (speed < -max) speed = -max;
-
-    //speed = -speed;
-
-
-    // find out the latency between incoming data and now:
-    float latency = get_ts_speed(e).get_last_timestamp().to_seconds() - humotion::Timestamp::now().to_seconds();
-    printf("TS DIFF %fms\n",latency*1000.0);
+    float vmax = 150.0;
+    if (target_velocity > vmax)  target_velocity = vmax;
+    if (target_velocity < -vmax) target_velocity = -vmax;
 
     //execute:
     //ivel->velocityMove(id, speed);
-    if ((id == ICUB_ID_NECK_PAN)  || (id == ICUB_ID_EYES_BOTH_UD) || (id == ICUB_ID_NECK_TILT) || (id == ICUB_ID_EYES_BOTH_UD) ||  (id == ICUB_ID_NECK_TILT) ){
-        //do a pd control for velocity moves that incorporates position errors:
-        humotion::Timestamp data_ts = get_ts_position(e).get_last_timestamp();
-        //TODO: add interpolation into future!
-        //humotion::Timestamp data_ts = humotion::Timestamp::now(); and extend get_interpol value with get_future_value
-        double position_error = target_angle[id] - get_ts_position(e).get_interpolated_value(data_ts);
-        double error_d = (position_error - last_position_error[id]) / (framerate*1000.0);
-        last_position_error[id] = position_error;
-        //finally do a PD loop to get the target velocity
-        double target_velocity = 0.1*(PID_P[id] * position_error + PID_D[id]*error_d) - speed;
-
-        printf("%f %f %f %f %f %f PID%d\n",
-               get_ts_position(e).get_interpolated_value(data_ts),
-               target_angle[id],
-               get_ts_speed(e).get_interpolated_value(data_ts),
-               target_velocity,
-               speed,
-               position_error,
-               id
-               );
-
-
-        //if (id == ICUB_ID_NECK_PAN) speed = -speed;
-        ivel->velocityMove(id, target_velocity);
-        printf("> VEL now=%3.2f target=%3.2f --> dist=%3.2f speed=%3.2f\n",target_angle_previous[id],target_angle[id],distance,speed);
+    if ((icub_id != ICUB_ID_NECK_PAN)  &&
+        (icub_id != ICUB_ID_EYES_BOTH_UD) &&
+        (icub_id != ICUB_ID_NECK_TILT) &&
+        (icub_id != ICUB_ID_EYES_BOTH_UD) &&
+        (icub_id != ICUB_ID_NECK_TILT)) {
+        // limit to some joints for debugging...
+        return;
     }
 
-    target_angle_previous[id] = get_ts_position(convert_motorid_to_enum(id)).get_newest_value();
+    // we now add a pd control loop for velocity moves in order to handle position errors
+    // TODO: add position interpolation into future. this requires to enable the velocity
+    //       broadcast in the torso and head ini file and fetch that velocity in the
+    //       icub_data_receiver as well. TODO: check if the can bus has enough bandwidth for that...
+
+    // first: fetch the timstamp of the last known position
+    humotion::Timestamp data_ts = get_ts_position(humotion_id).get_last_timestamp();
+
+    // calculate position error:
+    float position_error = target_angle_[icub_id] - get_ts_position(humotion_id).get_interpolated_value(data_ts);
+
+    // calculate d term
+    float error_d = (position_error - pv_mix_last_error_[icub_id]) / (framerate*1000.0);
+    pv_mix_last_error_[icub_id] = position_error;
+
+    // finally do a PD loop to get the target velocity
+    float pv_mix_velocity = pv_mix_pid_p_[icub_id] * position_error
+                            + pv_mix_pid_p_[icub_id]*error_d
+                            + target_velocity;
+
+    printf("%f %f %f %f %f %f PID%d\n",
+           get_ts_position(humotion_id).get_interpolated_value(data_ts),
+           target_angle_[icub_id],
+           123.4, //NOT USED ANYMORE//get_ts_speed(humotion_id).get_interpolated_value(data_ts),
+           pv_mix_velocity,
+           target_velocity,
+           position_error,
+           icub_id
+           );
+
+    // execute velocity move
+    yarp_ivel_->velocityMove(icub_id, pv_mix_velocity);
 }
 
 //! actually execute the scheduled motion commands
@@ -289,37 +290,32 @@ void iCubJointInterface::execute_motion(){
 
 
     //eyelids: unfortuantely the icub has only 1dof for eyelids, so we use only one dof here:
-    face_interface->set_eyelid_angle(target_angle[ICUB_ID_EYES_RIGHT_LID_UPPER]);
+    face_interface_->set_eyelid_angle(target_angle_[ICUB_ID_EYES_RIGHT_LID_UPPER]);
 
     //eyebrows are set using a special command as well:
-    face_interface->set_eyebrow_angle(ICUB_ID_EYES_LEFT_BROW, target_angle);
-    face_interface->set_eyebrow_angle(ICUB_ID_EYES_RIGHT_BROW, target_angle);
+    face_interface_->set_eyebrow_angle(ICUB_ID_EYES_LEFT_BROW, target_angle_);
+    face_interface_->set_eyebrow_angle(ICUB_ID_EYES_RIGHT_BROW, target_angle_);
 
     //mouth
-    face_interface->set_mouth(target_angle);
+    face_interface_->set_mouth(target_angle_);
 
 
     //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 timestamp = humotion::Timestamp::now().to_seconds();
+    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){
+void iCubJointInterface::store_incoming_position(int id, double value, double timestamp){
     //store joint based on id:
     switch(id){
         default:
@@ -374,59 +370,6 @@ void iCubJointInterface::fetch_position(int id, double value, double timestamp){
 
 }
 
-//! 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(ICUB_ID_NECK_PAN):
-            //PAN IS INVERTED
-            JointInterface::store_incoming_speed(ID_NECK_PAN, value, timestamp);
-            break;
-
-        case(ICUB_ID_NECK_TILT):
-            JointInterface::store_incoming_speed(ID_NECK_TILT, value, timestamp);
-            break;
-
-        case(ICUB_ID_NECK_ROLL):
-            JointInterface::store_incoming_speed(ID_NECK_ROLL, value, timestamp);
-            break;
-
-        case(ICUB_ID_EYES_BOTH_UD):
-            JointInterface::store_incoming_speed(ID_EYES_BOTH_UD, value, timestamp);
-            break;
-
-        //icub handles eyes differently, we have to set pan angle + vergence
-        case(ICUB_ID_EYES_PAN): {//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(ICUB_ID_EYES_VERGENCE): { //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;
-        }
-    }
-}
-
 void iCubJointInterface::set_joint_enable_state(int e, bool enable) {
     int icub_jointid = -1;
 
@@ -463,11 +406,11 @@ void iCubJointInterface::set_joint_enable_state(int e, bool enable) {
 
     if (icub_jointid != -1) {
         if (enable) {
-            amp->enableAmp(icub_jointid);
-            pid->enablePid(icub_jointid);
+            yarp_amp_->enableAmp(icub_jointid);
+            yarp_pid_->enablePid(icub_jointid);
         } else {
-            pid->disablePid(icub_jointid);
-            amp->disableAmp(icub_jointid);
+            yarp_pid_->disablePid(icub_jointid);
+            yarp_amp_->disableAmp(icub_jointid);
         }
     }
 }
@@ -495,15 +438,15 @@ void iCubJointInterface::store_min_max(IControlLimits *ilimits, int id, int e){
 //! initialise a joint (set up controller mode etc)
 //! \param joint enum
 void iCubJointInterface::init_joints(){
-    store_min_max(ilimits, ICUB_ID_NECK_TILT, ID_NECK_TILT);
-    store_min_max(ilimits, ICUB_ID_NECK_ROLL, ID_NECK_ROLL);
-    store_min_max(ilimits, ICUB_ID_NECK_PAN, ID_NECK_PAN);
-    store_min_max(ilimits, ICUB_ID_EYES_BOTH_UD, ID_EYES_BOTH_UD);
+    store_min_max(yarp_ilimits_, ICUB_ID_NECK_TILT, ID_NECK_TILT);
+    store_min_max(yarp_ilimits_, ICUB_ID_NECK_ROLL, ID_NECK_ROLL);
+    store_min_max(yarp_ilimits_, ICUB_ID_NECK_PAN, ID_NECK_PAN);
+    store_min_max(yarp_ilimits_, ICUB_ID_EYES_BOTH_UD, 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(ICUB_ID_EYES_PAN, &pan_min, &pan_max);
-    ilimits->getLimits(ICUB_ID_EYES_VERGENCE, &vergence_min, &vergence_max);
+    yarp_ilimits_->getLimits(ICUB_ID_EYES_PAN, &pan_min, &pan_max);
+    yarp_ilimits_->getLimits(ICUB_ID_EYES_VERGENCE, &vergence_min, &vergence_max);
 
     //this is not 100% correct, should be fixed:
     joint_min[ID_EYES_LEFT_LR] = pan_min; // - vergence_max/2;
@@ -535,6 +478,32 @@ void iCubJointInterface::init_joints(){
     joint_max[ID_LIP_RIGHT_UPPER] = 50;
     joint_min[ID_LIP_RIGHT_LOWER] = 5;
     joint_max[ID_LIP_RIGHT_LOWER] = 50;
+}
+
+//! conversion table for humotion joint id to icub joint id
+//! \param int value for humotion joint id from JointInterface::JOINT_ID_ENUM
+//! \return int value of icub joint id
+int iCubJointInterface::convert_humotion_jointid_to_icub(int humotion_id){
+    enum_id_bimap_t::right_const_iterator it = enum_id_bimap.right.find(humotion_id);
+    if(it == enum_id_bimap.right.end()) {
+        //key does not exist
+        cout << "ERROR: invalid humotion joint id given. can not convert this. exiting\n";
+        exit(EXIT_FAILURE);
+    }
+    return it->second;
+}
 
 
+//! conversion table for icub joint id to humotion joint id
+//! \param  int value of icub joint id
+//! \return int value of humotion joint id from JointInterface::JOINT_ID_ENUM
+int iCubJointInterface::convert_icub_jointid_to_humotion(int icub_id){
+    enum_id_bimap_t::left_const_iterator it = enum_id_bimap.left.find(icub_id);
+    if(it == enum_id_bimap.left.end()) {
+        //key does not exist
+        cout << "ERROR: invalid icub joint id given. can not convert this. exiting\n";
+        exit(EXIT_FAILURE);
+    }
+    return it->second;
 }
+
diff --git a/include/humotion/server/joint_interface.h b/include/humotion/server/joint_interface.h
index 1e74697..7bea418 100644
--- a/include/humotion/server/joint_interface.h
+++ b/include/humotion/server/joint_interface.h
@@ -50,9 +50,12 @@ public:
     JointInterface();
     ~JointInterface();
 
-    void set_target_position(int joint_id, float position, float speed);
+    void set_target(int joint_id, float position, float velocity);
     float get_target_position(int joint_id);
-    virtual void publish_target_position(int joint_id) = 0;
+    float get_target_velocity(int joint_id);
+
+    void publish_target(int joint_id);
+    virtual void publish_target(int joint_id, float position, float velocity) = 0;
     virtual void execute_motion() = 0;
 
     typedef std::map<int, TimestampedList> joint_tsl_map_t;
@@ -113,12 +116,13 @@ protected:
 
     float joint_min[JOINT_ID_ENUM_SIZE];
     float joint_max[JOINT_ID_ENUM_SIZE];
-    float joint_target[JOINT_ID_ENUM_SIZE];
-    float joint_target_speed[JOINT_ID_ENUM_SIZE];
 
 
 
 private:
+    float joint_target_position_[JOINT_ID_ENUM_SIZE];
+    float joint_target_velocity_[JOINT_ID_ENUM_SIZE];
+
     boost::mutex joint_ts_position_map_access_mutex;
     boost::mutex joint_ts_speed_map_access_mutex;
     joint_tsl_map_t joint_ts_position_map;
diff --git a/src/server/eye_motion_generator.cpp b/src/server/eye_motion_generator.cpp
index 0b22367..d89a716 100644
--- a/src/server/eye_motion_generator.cpp
+++ b/src/server/eye_motion_generator.cpp
@@ -124,15 +124,15 @@ void EyeMotionGenerator::calculate_targets(){
     reflexxes_calculate_profile();
 
     //tell the joint about the new values:
-    joint_interface->set_target_position(JointInterface::ID_EYES_LEFT_LR,
-                                         reflexxes_position_output->NewPositionVector->VecData[0],
-                                         reflexxes_position_output->NewVelocityVector->VecData[0]);
-    joint_interface->set_target_position(JointInterface::ID_EYES_RIGHT_LR,
-                                         reflexxes_position_output->NewPositionVector->VecData[1],
-                                         reflexxes_position_output->NewVelocityVector->VecData[1]);
-    joint_interface->set_target_position(JointInterface::ID_EYES_BOTH_UD,
-                                         reflexxes_position_output->NewPositionVector->VecData[2],
-                                         reflexxes_position_output->NewVelocityVector->VecData[2]);
+    joint_interface->set_target(JointInterface::ID_EYES_LEFT_LR,
+                                reflexxes_position_output->NewPositionVector->VecData[0],
+                                reflexxes_position_output->NewVelocityVector->VecData[0]);
+    joint_interface->set_target(JointInterface::ID_EYES_RIGHT_LR,
+                                reflexxes_position_output->NewPositionVector->VecData[1],
+                                reflexxes_position_output->NewVelocityVector->VecData[1]);
+    joint_interface->set_target(JointInterface::ID_EYES_BOTH_UD,
+                                reflexxes_position_output->NewPositionVector->VecData[2],
+                                reflexxes_position_output->NewVelocityVector->VecData[2]);
 
     cout << reflexxes_position_output->NewPositionVector->VecData[2] << " " << reflexxes_position_output->NewVelocityVector->VecData[2] ;
 }
@@ -142,8 +142,8 @@ void EyeMotionGenerator::calculate_targets(){
 void EyeMotionGenerator::publish_targets(){
     //publish values if there is an active gaze input within the last timerange
     if (gaze_target_input_active()){
-        joint_interface->publish_target_position(JointInterface::ID_EYES_LEFT_LR);
-        joint_interface->publish_target_position(JointInterface::ID_EYES_RIGHT_LR);
-        joint_interface->publish_target_position(JointInterface::ID_EYES_BOTH_UD);
+        joint_interface->publish_target(JointInterface::ID_EYES_LEFT_LR);
+        joint_interface->publish_target(JointInterface::ID_EYES_RIGHT_LR);
+        joint_interface->publish_target(JointInterface::ID_EYES_BOTH_UD);
     }
 }
diff --git a/src/server/eyebrow_motion_generator.cpp b/src/server/eyebrow_motion_generator.cpp
index dcc19ea..39445a2 100644
--- a/src/server/eyebrow_motion_generator.cpp
+++ b/src/server/eyebrow_motion_generator.cpp
@@ -22,15 +22,15 @@ void EyebrowMotionGenerator::calculate_targets(){
     float eyebrow_right_target = requested_gaze_state.eyebrow_right;
 
     //store targets:
-    joint_interface->set_target_position(JointInterface::ID_EYES_LEFT_BROW, eyebrow_left_target, 0.0);
-    joint_interface->set_target_position(JointInterface::ID_EYES_RIGHT_BROW, eyebrow_right_target, 0.0);
+    joint_interface->set_target(JointInterface::ID_EYES_LEFT_BROW, eyebrow_left_target, 0.0);
+    joint_interface->set_target(JointInterface::ID_EYES_RIGHT_BROW, eyebrow_right_target, 0.0);
 }
 
 //! publish targets to motor boards:
 void EyebrowMotionGenerator::publish_targets(){
     //publish values if there is an active gaze input within the last timerange
     if (gaze_target_input_active()){
-        joint_interface->publish_target_position(JointInterface::ID_EYES_LEFT_BROW);
-        joint_interface->publish_target_position(JointInterface::ID_EYES_RIGHT_BROW);
+        joint_interface->publish_target(JointInterface::ID_EYES_LEFT_BROW);
+        joint_interface->publish_target(JointInterface::ID_EYES_RIGHT_BROW);
     }
 }
diff --git a/src/server/eyelid_motion_generator.cpp b/src/server/eyelid_motion_generator.cpp
index e3eba0f..c5af433 100644
--- a/src/server/eyelid_motion_generator.cpp
+++ b/src/server/eyelid_motion_generator.cpp
@@ -77,14 +77,14 @@ void EyelidMotionGenerator::calculate_targets(){
     //limit target angles:
     eyelid_upper_left_target  = limit_target(JointInterface::ID_EYES_LEFT_LID_UPPER,  eyelid_upper_left_target);
     eyelid_lower_left_target  = limit_target(JointInterface::ID_EYES_LEFT_LID_LOWER,  eyelid_lower_left_target);
-    eyelid_upper_right_target = limit_target(JointInterface::ID_EYES_RIGHT_LID_UPPER,  eyelid_upper_right_target);
-    eyelid_lower_right_target = limit_target(JointInterface::ID_EYES_RIGHT_LID_LOWER,  eyelid_lower_right_target);
+    eyelid_upper_right_target = limit_target(JointInterface::ID_EYES_RIGHT_LID_UPPER, eyelid_upper_right_target);
+    eyelid_lower_right_target = limit_target(JointInterface::ID_EYES_RIGHT_LID_LOWER, eyelid_lower_right_target);
 
     //(temporarily) store the target
-    joint_interface->set_target_position(JointInterface::ID_EYES_LEFT_LID_UPPER,  eyelid_upper_left_target, 0.0);
-    joint_interface->set_target_position(JointInterface::ID_EYES_LEFT_LID_LOWER,  eyelid_lower_left_target, 0.0);
-    joint_interface->set_target_position(JointInterface::ID_EYES_RIGHT_LID_UPPER,  eyelid_upper_right_target, 0.0);
-    joint_interface->set_target_position(JointInterface::ID_EYES_RIGHT_LID_LOWER,  eyelid_lower_right_target, 0.0);
+    joint_interface->set_target(JointInterface::ID_EYES_LEFT_LID_UPPER,  eyelid_upper_left_target, 0.0);
+    joint_interface->set_target(JointInterface::ID_EYES_LEFT_LID_LOWER,  eyelid_lower_left_target, 0.0);
+    joint_interface->set_target(JointInterface::ID_EYES_RIGHT_LID_UPPER, eyelid_upper_right_target, 0.0);
+    joint_interface->set_target(JointInterface::ID_EYES_RIGHT_LID_LOWER, eyelid_lower_right_target, 0.0);
 
     //check for saccade
     check_for_saccade();
@@ -221,7 +221,7 @@ void EyelidMotionGenerator::close_eyelid(int joint_id){
         //use the upper value + 10 deg as close state:
         value = joint_interface->get_joint_min(JointInterface::ID_EYES_LEFT_LID_UPPER) + 10.0;
         //overwrite last target_
-        joint_interface->set_target_position(joint_id, value, 0.0);
+        joint_interface->set_target(joint_id, value, 0.0);
         break;
 
     case(JointInterface::ID_EYES_RIGHT_LID_UPPER):
@@ -229,7 +229,7 @@ void EyelidMotionGenerator::close_eyelid(int joint_id){
         //use the upper value + 10 deg as close state:
         value = joint_interface->get_joint_min(JointInterface::ID_EYES_RIGHT_LID_UPPER) + 10.0;
         //overwrite last target_
-        joint_interface->set_target_position(joint_id, value, 0.0);
+        joint_interface->set_target(joint_id, value, 0.0);
         break;
     }
 }
@@ -268,9 +268,9 @@ void EyelidMotionGenerator::check_for_saccade(){
 void EyelidMotionGenerator::publish_targets(){
     //publish values if there is an active gaze input within the last timerange
     if (gaze_target_input_active()){
-        joint_interface->publish_target_position(JointInterface::ID_EYES_LEFT_LID_UPPER);
-        joint_interface->publish_target_position(JointInterface::ID_EYES_LEFT_LID_LOWER);
-        joint_interface->publish_target_position(JointInterface::ID_EYES_RIGHT_LID_UPPER);
-        joint_interface->publish_target_position(JointInterface::ID_EYES_RIGHT_LID_LOWER);
+        joint_interface->publish_target(JointInterface::ID_EYES_LEFT_LID_UPPER);
+        joint_interface->publish_target(JointInterface::ID_EYES_LEFT_LID_LOWER);
+        joint_interface->publish_target(JointInterface::ID_EYES_RIGHT_LID_UPPER);
+        joint_interface->publish_target(JointInterface::ID_EYES_RIGHT_LID_LOWER);
     }
 }
diff --git a/src/server/joint_interface.cpp b/src/server/joint_interface.cpp
index 52ec3ea..006030d 100644
--- a/src/server/joint_interface.cpp
+++ b/src/server/joint_interface.cpp
@@ -48,19 +48,26 @@ JointInterface::~JointInterface(){
 //! set joint target position
 //! \param joint_id of joint
 //! \param float position
-void JointInterface::set_target_position(int joint_id, float position, float speed){
+void JointInterface::set_target(int joint_id, float position, float velocity){
     assert(joint_id < JOINT_ID_ENUM_SIZE);
 
     //update current value
-    joint_target[joint_id] = position;
-    joint_target_speed[joint_id] = speed;
+    joint_target_position_[joint_id] = position;
+    joint_target_velocity_[joint_id] = velocity;
 }
 
 //! fetch target position
 //! \param joint_id of joint
 float JointInterface::get_target_position(int joint_id){
     assert(joint_id < JOINT_ID_ENUM_SIZE);
-    return joint_target[joint_id];
+    return joint_target_position_[joint_id];
+}
+
+//! fetch target velocity
+//! \param joint_id of joint
+float JointInterface::get_target_velocity(int joint_id){
+    assert(joint_id < JOINT_ID_ENUM_SIZE);
+    return joint_target_velocity_[joint_id];
 }
 
 //! incoming position data
@@ -234,14 +241,14 @@ void JointInterface::disable_gaze_joints(){
     gaze_enabled = false;
 }
 
-//! fetch maximum allowable joint position
+//! fetch maximum allowed joint position
 //! \return max position
 float JointInterface::get_joint_max(int joint_id){
     assert((joint_id > 0) && (joint_id < JOINT_ID_ENUM_SIZE));
     return joint_max[joint_id];
 }
 
-//! fetch minimum allowable joint position
+//! fetch minimum allowed joint position
 //! \return min position
 float JointInterface::get_joint_min(int joint_id){
     assert((joint_id > 0) && (joint_id < JOINT_ID_ENUM_SIZE));
@@ -253,3 +260,8 @@ float JointInterface::get_joint_min(int joint_id){
 bool JointInterface::get_joint_position_map_empty(){
     return (joint_ts_position_map.empty());
 }
+
+//! call the virtual store function with given position and velocities
+void JointInterface::publish_target(int joint_id) {
+    publish_target(joint_id, joint_target_position_[joint_id], joint_target_velocity_[joint_id]);
+}
diff --git a/src/server/mouth_motion_generator.cpp b/src/server/mouth_motion_generator.cpp
index 1faecb8..cc6a3e5 100644
--- a/src/server/mouth_motion_generator.cpp
+++ b/src/server/mouth_motion_generator.cpp
@@ -91,11 +91,9 @@ void MouthMotionGenerator::update_mouth_target(int upper_id, int lower_id){
         unsafe_target_upper = unsafe_target_lower - opening;
     }
 
-    //tell the joint about the new values:
-    joint_interface->set_target_position(upper_id, unsafe_target_upper, 0.0);
-    joint_interface->set_target_position(lower_id, unsafe_target_lower, 0.0);
-
-    //printf("> update_mouth_target(u=%d, l=%d) -> upper = %4.2f, lower = %4.2f\n",upper_id, lower_id,unsafe_target_upper,unsafe_target_lower);
+    // tell the joint about the new values:
+    joint_interface->set_target(upper_id, unsafe_target_upper, 0.0);
+    joint_interface->set_target(lower_id, unsafe_target_lower, 0.0);
 }
 
 
@@ -139,12 +137,12 @@ float MouthMotionGenerator::mouthstate_to_position(MouthState m, int e){
 void MouthMotionGenerator::publish_targets(){
     //publish values if there is an active gaze input within the last timerange
     if (mouth_target_input_active()){
-        joint_interface->publish_target_position(JointInterface::ID_LIP_LEFT_UPPER);
-        joint_interface->publish_target_position(JointInterface::ID_LIP_LEFT_LOWER);
-        joint_interface->publish_target_position(JointInterface::ID_LIP_CENTER_UPPER);
-        joint_interface->publish_target_position(JointInterface::ID_LIP_CENTER_LOWER);
-        joint_interface->publish_target_position(JointInterface::ID_LIP_RIGHT_UPPER);
-        joint_interface->publish_target_position(JointInterface::ID_LIP_RIGHT_LOWER);
+        joint_interface->publish_target(JointInterface::ID_LIP_LEFT_UPPER);
+        joint_interface->publish_target(JointInterface::ID_LIP_LEFT_LOWER);
+        joint_interface->publish_target(JointInterface::ID_LIP_CENTER_UPPER);
+        joint_interface->publish_target(JointInterface::ID_LIP_CENTER_LOWER);
+        joint_interface->publish_target(JointInterface::ID_LIP_RIGHT_UPPER);
+        joint_interface->publish_target(JointInterface::ID_LIP_RIGHT_LOWER);
     }
 }
 
diff --git a/src/server/neck_motion_generator.cpp b/src/server/neck_motion_generator.cpp
index 0494e40..cf45974 100644
--- a/src/server/neck_motion_generator.cpp
+++ b/src/server/neck_motion_generator.cpp
@@ -162,17 +162,17 @@ void NeckMotionGenerator::calculate_targets(){
     reflexxes_calculate_profile();
 
     //tell the joint if about the new values:
-    joint_interface->set_target_position(JointInterface::ID_NECK_PAN,
-                                         reflexxes_position_output->NewPositionVector->VecData[0],
-                                         reflexxes_position_output->NewVelocityVector->VecData[0]);
+    joint_interface->set_target(JointInterface::ID_NECK_PAN,
+                                reflexxes_position_output->NewPositionVector->VecData[0],
+                                reflexxes_position_output->NewVelocityVector->VecData[0]);
 
-    joint_interface->set_target_position(JointInterface::ID_NECK_TILT,
-                                         reflexxes_position_output->NewPositionVector->VecData[1],
-                                         reflexxes_position_output->NewVelocityVector->VecData[1]);
+    joint_interface->set_target(JointInterface::ID_NECK_TILT,
+                                reflexxes_position_output->NewPositionVector->VecData[1],
+                                reflexxes_position_output->NewVelocityVector->VecData[1]);
 
-    joint_interface->set_target_position(JointInterface::ID_NECK_ROLL,
-                                         reflexxes_position_output->NewPositionVector->VecData[2],
-                                         reflexxes_position_output->NewVelocityVector->VecData[2]);
+    joint_interface->set_target(JointInterface::ID_NECK_ROLL,
+                                reflexxes_position_output->NewPositionVector->VecData[2],
+                                reflexxes_position_output->NewVelocityVector->VecData[2]);
 
     printf("\n%f %f %f %f %f DBG\n",
             neck_pan_now, neck_pan_target,
@@ -186,9 +186,9 @@ void NeckMotionGenerator::calculate_targets(){
 void NeckMotionGenerator::publish_targets(){
     //publish values if there is an active gaze input within the last timerange
     if (gaze_target_input_active()){
-        joint_interface->publish_target_position(JointInterface::ID_NECK_PAN);
-        joint_interface->publish_target_position(JointInterface::ID_NECK_TILT);
-        joint_interface->publish_target_position(JointInterface::ID_NECK_ROLL);
+        joint_interface->publish_target(JointInterface::ID_NECK_PAN);
+        joint_interface->publish_target(JointInterface::ID_NECK_TILT);
+        joint_interface->publish_target(JointInterface::ID_NECK_ROLL);
     }
 }
 
