commit 0d0f5ca1125e841198d069282cd41adaf77ad765
Author: Simon Schulz <sschulz@cit-ec.uni-bielefeld.de>
Date:   Fri Feb 5 10:33:18 2016 +0100

    testing

diff --git a/examples/yarp_icub/CMakeLists.txt b/examples/yarp_icub/CMakeLists.txt
index bf9b861..791fcfa 100644
--- a/examples/yarp_icub/CMakeLists.txt
+++ b/examples/yarp_icub/CMakeLists.txt
@@ -18,7 +18,7 @@ SET(MAIN icub_humotion_server)
 
 file(GLOB DUMMY_HEADER_LIST RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} include/*.h)
 
-ADD_EXECUTABLE(${MAIN} src/main.cpp src/icub_jointinterface.cpp src/icub_data_receiver.cpp ${DUMMY_HEADER_LIST})
+ADD_EXECUTABLE(${MAIN} src/main.cpp src/icub_jointinterface.cpp src/icub_faceinterface.cpp src/icub_data_receiver.cpp ${DUMMY_HEADER_LIST})
 
 # we now add the YARP and iCub libraries to our project.
 TARGET_LINK_LIBRARIES(${MAIN} ${Boost_LIBRARIES} ${YARP_LIBRARIES} ${ICUB_LIBRARIES} ${humotion_LIBRARIES})
diff --git a/examples/yarp_icub/include/icub_jointinterface.h b/examples/yarp_icub/include/icub_jointinterface.h
index 5064b51..7d4ba39 100644
--- a/examples/yarp_icub/include/icub_jointinterface.h
+++ b/examples/yarp_icub/include/icub_jointinterface.h
@@ -18,6 +18,7 @@
 #include "icub_data_receiver.h"
 
 class iCubDataReceiver;
+class iCubFaceInterface;
 
 class iCubJointInterface : public humotion::server::JointInterface{
 public:
@@ -69,15 +70,8 @@ private:
     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;
     yarp::dev::PolyDriver dd;
@@ -85,8 +79,6 @@ private:
     yarp::sig::Vector velocities;
     yarp::sig::Vector commands;
 
-    yarp::os::BufferedPort<yarp::os::Bottle> emotion_port[4];
-
     yarp::dev::IVelocityControl *ivel;
     yarp::dev::IPositionControl *ipos;
     yarp::dev::IEncodersTimed *iencs;
@@ -110,6 +102,7 @@ private:
     int convert_enum_to_motorid(int e);
     int convert_motorid_to_enum(int id);
 
+    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 628c737..8db2884 100644
--- a/examples/yarp_icub/src/icub_data_receiver.cpp
+++ b/examples/yarp_icub/src/icub_data_receiver.cpp
@@ -38,6 +38,9 @@ void iCubDataReceiver::run(){
         icub_jointinterface->fetch_speed(i, velocities[i], 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());
     //fixme: use real id
diff --git a/examples/yarp_icub/src/icub_jointinterface.cpp b/examples/yarp_icub/src/icub_jointinterface.cpp
index 09cfee8..463c631 100644
--- a/examples/yarp_icub/src/icub_jointinterface.cpp
+++ b/examples/yarp_icub/src/icub_jointinterface.cpp
@@ -1,4 +1,6 @@
 #include "icub_jointinterface.h"
+#include "icub_faceinterface.h"
+
 #include <yarp/os/Property.h>
 using namespace yarp::dev;
 using namespace yarp::sig;
@@ -22,7 +24,7 @@ http://wiki.icub.org/wiki/Motor_control
 //! 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
@@ -99,22 +101,6 @@ iCubJointInterface::iCubJointInterface(string _scope) : humotion::server::JointI
         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
@@ -148,97 +134,9 @@ int iCubJointInterface::convert_motorid_to_enum(int id){
     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].writeStrict();
-    }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].writeStrict();
-    }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);
+    iCubDataReceiver *data_receiver = new iCubDataReceiver(1000.0 / MAIN_LOOP_FREQUENCY, iencs, this);
     data_receiver->start();
 }
 
@@ -248,6 +146,7 @@ void iCubJointInterface::run(){
 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
     }
@@ -273,9 +172,8 @@ void iCubJointInterface::publish_target_position(int e){
 //! \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);
+    printf("> set joint %d = %f\n",id,value);
     target_angle[id] = value;
-    //ipos->positionMove(id, value);
 }
 
 //! execute a move in position mode
@@ -323,7 +221,7 @@ void iCubJointInterface::set_target_in_velocitymode(int id, double value){
     //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) ){
-        if (id == ICUB_ID_NECK_PAN) speed = -speed;
+        //if (id == ICUB_ID_NECK_PAN) speed = -speed;
         ivel->velocityMove(id, speed);
         printf("> VEL now=%3.2f target=%3.2f --> dist=%3.2f speed=%3.2f\n",target_angle_previous[id],value,distance,speed);
     }
@@ -350,88 +248,14 @@ void iCubJointInterface::execute_motion(){
 
 
     //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]);
+    face_interface->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);
+    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
-    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].writeStrict();
+    face_interface->set_mouth(target_angle);
 
 
     //store joint values which we do not handle on icub here:
@@ -442,7 +266,6 @@ void iCubJointInterface::set_mouth(){
     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(){
@@ -463,12 +286,12 @@ void iCubJointInterface::fetch_position(int id, double value, double timestamp){
             return;
 
         case(100):
-            JointInterface::store_incoming_position(ID_EYES_RIGHT_LID_UPPER, lid_angle, timestamp);
+            //JointInterface::store_incoming_position(ID_EYES_RIGHT_LID_UPPER, lid_angle, timestamp);
             break;
 
         case(ICUB_ID_NECK_PAN):
             //PAN is inverted!
-            JointInterface::store_incoming_position(ID_NECK_PAN, -value, timestamp);
+            JointInterface::store_incoming_position(ID_NECK_PAN, value, timestamp);
             break;
 
         case(ICUB_ID_NECK_TILT):
@@ -523,7 +346,7 @@ void iCubJointInterface::fetch_speed(int id, double value, double timestamp){
 
         case(ICUB_ID_NECK_PAN):
             //PAN IS INVERTED
-            JointInterface::store_incoming_speed(ID_NECK_PAN, -value, timestamp);
+            JointInterface::store_incoming_speed(ID_NECK_PAN, value, timestamp);
             break;
 
         case(ICUB_ID_NECK_TILT):
@@ -650,7 +473,7 @@ void iCubJointInterface::init_joints(){
     //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];
+    //lid_angle = joint_max[ID_EYES_RIGHT_LID_UPPER];
 
     //eyebrows:
     joint_min[ID_EYES_LEFT_BROW] = -50;
diff --git a/include/humotion/server/eye_motion_generator.h b/include/humotion/server/eye_motion_generator.h
index 7b38638..cbc0832 100644
--- a/include/humotion/server/eye_motion_generator.h
+++ b/include/humotion/server/eye_motion_generator.h
@@ -43,7 +43,7 @@ public:
 
     static const float SACCADE_SPEED_THRESHOLD;
 private:
-    void setup_eyemotion(int dof, float target, float now);
+    void setup_eyemotion(int dof, float target, float current_position, float current_speed);
 
     TimestampedList tsl_gaze_target_pan;
     TimestampedList tsl_gaze_target_tilt;
diff --git a/include/humotion/server/motion_generator.h b/include/humotion/server/motion_generator.h
index 7c4f75a..2a4b85d 100644
--- a/include/humotion/server/motion_generator.h
+++ b/include/humotion/server/motion_generator.h
@@ -46,6 +46,7 @@ public:
 
 protected:
     float get_current_position(int joint_id);
+    float get_current_speed(int joint_id);
     float limit_target(int joint_id, float val);
 
     JointInterface *joint_interface;
diff --git a/include/humotion/server/neck_motion_generator.h b/include/humotion/server/neck_motion_generator.h
index c8078a2..87e4afd 100644
--- a/include/humotion/server/neck_motion_generator.h
+++ b/include/humotion/server/neck_motion_generator.h
@@ -37,7 +37,7 @@ public:
     ~NeckMotionGenerator();
     void calculate_targets();
     void publish_targets();
-    void setup_neckmotion(int dof, float target, float now);
+    void setup_neckmotion(int dof, float target, float current_position, float current_speed);
 
 private:
     float get_breath_offset();
diff --git a/include/humotion/server/reflexxes_motion_generator.h b/include/humotion/server/reflexxes_motion_generator.h
index 525a48a..da33839 100644
--- a/include/humotion/server/reflexxes_motion_generator.h
+++ b/include/humotion/server/reflexxes_motion_generator.h
@@ -41,7 +41,7 @@ public:
     ~ReflexxesMotionGenerator();
 
 protected:
-    void reflexxes_set_input(int dof, float target, float position, float max_speed, float max_accel);
+    void reflexxes_set_input(int dof, float target, float current_position, float current_speed, float max_speed, float max_accel);
     void reflexxes_calculate_profile();
 
     //*****************************************
diff --git a/src/server/controller.cpp b/src/server/controller.cpp
index b3b7b5d..c45a860 100644
--- a/src/server/controller.cpp
+++ b/src/server/controller.cpp
@@ -98,7 +98,7 @@ GazeState Controller::relative_gaze_to_absolute_gaze(GazeState relative){
 
     //check if this timestamp allows a valid conversion:
     Timestamp history_begin = joint_interface->get_ts_position(JointInterface::ID_NECK_PAN).get_first_timestamp();
-    Timestamp history_end   = joint_interface->get_ts_position(JointInterface::ID_NECK_PAN).get_last_timestamp();
+    // Timestamp history_end   = joint_interface->get_ts_position(JointInterface::ID_NECK_PAN).get_last_timestamp();
 
     //printf("> incoming: %f, history is %f to %f\n",relative_target_timestamp.to_seconds(), history_begin.to_seconds(), history_end.to_seconds());
 
diff --git a/src/server/eye_motion_generator.cpp b/src/server/eye_motion_generator.cpp
index 1572199..98df32b 100644
--- a/src/server/eye_motion_generator.cpp
+++ b/src/server/eye_motion_generator.cpp
@@ -49,9 +49,9 @@ EyeMotionGenerator::~EyeMotionGenerator(){
 //! \param dof id of joint
 //! \param target angle
 //! \param current angle
-void EyeMotionGenerator::setup_eyemotion(int dof, float target, float now){
+void EyeMotionGenerator::setup_eyemotion(int dof, float target, float current_position, float current_speed){
     //get distance to target:
-    float distance_abs = fabs(target - now);
+    float distance_abs = fabs(target - current_position);
     //get max speed: factor can be found in encyc britannica: "linear.... being 300° per second for 10° and 500° per second for 30°" (max=700)
     float max_speed = fmin(700.0, 10.0*distance_abs + 200.0);
     //max accel: use data from ""Speed and Accuracy of Saccadic Eye Movements: Characteristics of Impulse Variability in the Oculomotor System"
@@ -60,7 +60,7 @@ void EyeMotionGenerator::setup_eyemotion(int dof, float target, float now){
     float max_accel = fmin(80000.0, 1526.53*distance_abs + 10245.4);
 
     //feed reflexxes api with data
-    reflexxes_set_input(dof, target, now, max_speed, max_accel);
+    reflexxes_set_input(dof, target, current_position, current_speed, max_speed, max_accel);
 }
 
 //! calculate joint targets
@@ -95,12 +95,17 @@ void EyeMotionGenerator::calculate_targets(){
     float eye_pan_l_now = get_current_position(JointInterface::ID_EYES_LEFT_LR);
     float eye_pan_r_now = get_current_position(JointInterface::ID_EYES_RIGHT_LR);
     float eye_tilt_now  = get_current_position(JointInterface::ID_EYES_BOTH_UD);
+    //fetch current velocities
+    float eye_pan_l_speed = get_current_speed(JointInterface::ID_EYES_LEFT_LR);
+    float eye_pan_r_speed = get_current_speed(JointInterface::ID_EYES_RIGHT_LR);
+    float eye_tilt_speed  = get_current_speed(JointInterface::ID_EYES_BOTH_UD);
 
     //pass paramaters to reflexxes api:
-    setup_eyemotion(0, eye_pan_l_target, eye_pan_l_now);
-    setup_eyemotion(1, eye_pan_r_target, eye_pan_r_now);
-    setup_eyemotion(2, eye_tilt_target, eye_tilt_now);
-    cout << "EYE MOTION 2 " << eye_tilt_target << " now=" << eye_tilt_now << "\n";
+    setup_eyemotion(0, eye_pan_l_target, eye_pan_l_now, eye_pan_l_speed);
+    setup_eyemotion(1, eye_pan_r_target, eye_pan_r_now, eye_pan_r_speed);
+    setup_eyemotion(2, eye_tilt_target, eye_tilt_now, eye_tilt_speed);
+
+    // cout << "EYE MOTION 2 " << eye_tilt_target << " now=" << eye_tilt_now << "\n";
 
     //call reflexxes to handle profile calculation:
     reflexxes_calculate_profile();
diff --git a/src/server/motion_generator.cpp b/src/server/motion_generator.cpp
index b70e0a5..a8099b1 100644
--- a/src/server/motion_generator.cpp
+++ b/src/server/motion_generator.cpp
@@ -45,7 +45,14 @@ MotionGenerator::MotionGenerator(JointInterface *j){
 MotionGenerator::~MotionGenerator(){
 }
 
-//! fetch thr latest (=current) position of a joint
+//! fetch the latest (=current) speed of a joint
+//! \param joint_id
+//! \return float value of joint speed
+float MotionGenerator::get_current_speed(int joint_id){
+    return joint_interface->get_ts_speed(joint_id).get_newest_value();
+}
+
+//! fetch the latest (=current) position of a joint
 //! \param joint_id
 //! \return float value of joint position
 float MotionGenerator::get_current_position(int joint_id){
diff --git a/src/server/neck_motion_generator.cpp b/src/server/neck_motion_generator.cpp
index 61dfbdf..3a0c344 100644
--- a/src/server/neck_motion_generator.cpp
+++ b/src/server/neck_motion_generator.cpp
@@ -95,6 +95,10 @@ void NeckMotionGenerator::calculate_targets(){
     float neck_pan_now  = get_current_position(JointInterface::ID_NECK_PAN);
     float neck_tilt_now = get_current_position(JointInterface::ID_NECK_TILT);
     float neck_roll_now = get_current_position(JointInterface::ID_NECK_ROLL);
+    // fetch current velocities
+    float neck_pan_speed  = get_current_speed(JointInterface::ID_NECK_PAN);
+    float neck_tilt_speed = get_current_speed(JointInterface::ID_NECK_TILT);
+    float neck_roll_speed = get_current_speed(JointInterface::ID_NECK_ROLL);
 
     //reached target?
     float goal_diff   = fabs(get_current_gaze().distance_pt_abs(requested_gaze_state));
@@ -143,9 +147,9 @@ void NeckMotionGenerator::calculate_targets(){
     neck_tilt_target += get_breath_offset();
 
     //pass parameters to reflexxes api:
-    setup_neckmotion(0, neck_pan_target,  neck_pan_now);
-    setup_neckmotion(1, neck_tilt_target, neck_tilt_now);
-    setup_neckmotion(2, neck_roll_target, neck_roll_now);
+    setup_neckmotion(0, neck_pan_target,  neck_pan_now, neck_pan_speed);
+    setup_neckmotion(1, neck_tilt_target, neck_tilt_now, neck_tilt_speed);
+    setup_neckmotion(2, neck_roll_target, neck_roll_now, neck_roll_speed);
 
     //call reflexxes to handle profile calculation:
     reflexxes_calculate_profile();
@@ -158,9 +162,17 @@ void NeckMotionGenerator::calculate_targets(){
     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_position(JointInterface::ID_NECK_ROLL,
                                          reflexxes_position_output->NewPositionVector->VecData[2],
                                          reflexxes_position_output->NewVelocityVector->VecData[2]);
+
+    printf("\n%f %f %f %f %f %f DBG\n",
+            neck_pan_now, neck_pan_target,
+            reflexxes_position_output->NewPositionVector->VecData[0],
+            joint_interface->get_ts_speed(JointInterface::ID_NECK_PAN).get_newest_value(),
+            reflexxes_position_output->NewVelocityVector->VecData[0]
+            );
 }
 
 //! publish targets to motor boards:
@@ -179,9 +191,9 @@ void NeckMotionGenerator::publish_targets(){
 //! \param dof id of joint
 //! \param target angle
 //! \param current angle
-void NeckMotionGenerator::setup_neckmotion(int dof, float target, float now){
+void NeckMotionGenerator::setup_neckmotion(int dof, float target, float current_position, float current_speed){
     //get distance to target:
-    float distance_abs = fabs(target - now);
+    float distance_abs = fabs(target - current_position);
 
     //get max speed: according to [guitton87] there is a relation between distance_abs and v_max_head:
     //v_max = 4.39 * d_total + 106.0 (in degrees)
@@ -216,5 +228,5 @@ void NeckMotionGenerator::setup_neckmotion(int dof, float target, float now){
     ///printf("MAX SPEED %4.2f / max accel %4.2f\n",max_speed, max_accel);
 
     //feed reflexxes api with data
-    reflexxes_set_input(dof, target, now, max_speed, max_accel);
+    reflexxes_set_input(dof, target, current_position, current_speed, max_speed, max_accel);
 }
diff --git a/src/server/reflexxes_motion_generator.cpp b/src/server/reflexxes_motion_generator.cpp
index eb2bd20..93e9c90 100644
--- a/src/server/reflexxes_motion_generator.cpp
+++ b/src/server/reflexxes_motion_generator.cpp
@@ -54,7 +54,7 @@ ReflexxesMotionGenerator::~ReflexxesMotionGenerator(){
 //! \param target angle
 //! \param max_speed max reachable speed during accel
 //! \param max_accel max allowable acceleration
-void ReflexxesMotionGenerator::reflexxes_set_input(int dof, float target, float position, float max_speed, float max_accel){
+void ReflexxesMotionGenerator::reflexxes_set_input(int dof, float target, float current_position, float current_speed, float max_speed, float max_accel){
     assert(dof < dof_count);
 
     //set up reflexxes:
@@ -63,15 +63,17 @@ void ReflexxesMotionGenerator::reflexxes_set_input(int dof, float target, float
     reflexxes_position_input->MaxVelocityVector->VecData[dof]     = max_speed;
     reflexxes_position_input->MaxAccelerationVector->VecData[dof] = max_accel;
     reflexxes_position_input->TargetVelocityVector->VecData[dof]  = 0.0; //target speed is zero (really?)
-    reflexxes_position_input->CurrentPositionVector->VecData[dof]  = position;
+    // feed back current pos & velocity
+    reflexxes_position_input->CurrentPositionVector->VecData[dof]  = current_position;
+    reflexxes_position_input->CurrentVelocityVector->VecData[dof]  = current_speed;
 
     // safety: libreflexxes does not like zero accellerations...
     if (reflexxes_position_input->MaxAccelerationVector->VecData[dof] == 0.0){
             reflexxes_position_input->MaxAccelerationVector->VecData[dof] = 0.0001;
     }
-
 }
 
+
 //! calculate motion profile
 
 void ReflexxesMotionGenerator::reflexxes_calculate_profile(){
@@ -88,7 +90,7 @@ void ReflexxesMotionGenerator::reflexxes_calculate_profile(){
     //feed back values:
     for(int i=0; i<dof_count; i++){
         //reflexxes_position_input->CurrentPositionVector->VecData[i]     = reflexxes_position_output->NewPositionVector->VecData[i];
-        reflexxes_position_input->CurrentVelocityVector->VecData[i]     = reflexxes_position_output->NewVelocityVector->VecData[i];
-        reflexxes_position_input->CurrentAccelerationVector->VecData[i] = reflexxes_position_output->NewAccelerationVector->VecData[i];
+        //reflexxes_position_input->CurrentVelocityVector->VecData[i]     = reflexxes_position_output->NewVelocityVector->VecData[i];
+        //reflexxes_position_input->CurrentAccelerationVector->VecData[i] = reflexxes_position_output->NewAccelerationVector->VecData[i];
     }
 }
