commit 87b509882a10be4ebe6029645ec1e9b13f4d2f32
Author: Simon Schulz <sschulz@cit-ec.uni-bielefeld.de>
Date:   Tue Feb 9 16:00:09 2016 +0100

    testing delay

diff --git a/examples/yarp_icub/CMakeLists.txt.user b/examples/yarp_icub/CMakeLists.txt.user
index be1bbb0..a2e29f2 100644
--- a/examples/yarp_icub/CMakeLists.txt.user
+++ b/examples/yarp_icub/CMakeLists.txt.user
@@ -1,6 +1,6 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <!DOCTYPE QtCreatorProject>
-<!-- Written by QtCreator 3.0.1, 2016-02-04T09:55:06. -->
+<!-- Written by QtCreator 3.0.1, 2016-02-09T13:32:45. -->
 <qtcreator>
  <data>
   <variable>ProjectExplorer.Project.ActiveTarget</variable>
diff --git a/examples/yarp_icub/include/icub_jointinterface.h b/examples/yarp_icub/include/icub_jointinterface.h
index 7d4ba39..1a574e2 100644
--- a/examples/yarp_icub/include/icub_jointinterface.h
+++ b/examples/yarp_icub/include/icub_jointinterface.h
@@ -86,6 +86,9 @@ private:
     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);
 
@@ -95,8 +98,8 @@ private:
     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);
+    void set_target_in_positionmode(int id);
+    void set_target_in_velocitymode(int id);
 
 
     int convert_enum_to_motorid(int e);
diff --git a/examples/yarp_icub/src/icub_jointinterface.cpp b/examples/yarp_icub/src/icub_jointinterface.cpp
index 33bf3bb..643951d 100644
--- a/examples/yarp_icub/src/icub_jointinterface.cpp
+++ b/examples/yarp_icub/src/icub_jointinterface.cpp
@@ -60,6 +60,15 @@ iCubJointInterface::iCubJointInterface(string _scope) : humotion::server::JointI
     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::right_const_iterator it;
+    for(it = enum_id_bimap.left.begin(); it == enum_id_bimap.left.end(); it++) {
+        last_position_error[*it] = 0.0;
+        PID_P[id] = 0.5;
+        PID_D[id] = 0.1;
+    }
+
+
     Property options;
     options.put("device", "remote_controlboard");
     options.put("local", "/local/head");
@@ -179,21 +188,23 @@ void iCubJointInterface::store_joint(int id, float value){
 //! execute a move in position mode
 //! \param id of joint
 //! \param angle
-void iCubJointInterface::set_target_in_positionmode(int id, double value){
+void iCubJointInterface::set_target_in_positionmode(int id){
+    double target = target_angle[id];
+
     if (id>ICUB_ID_EYES_VERGENCE){
-        printf("> ERROR: set_target_positionmode(id=%d, %3.2f) not supported for this id\n",id,value);
+        printf("> ERROR: set_target_positionmode(id=%d, %3.2f) not supported for this id\n",id,target);
         return;
     }
 
     // execute motion as position control cmd
-    ipos->positionMove(id, value);
+    ipos->positionMove(id, target);
 
 }
 
 //! execute a move in velocity mode
 //! \param id of joint
 //! \param angle
-void iCubJointInterface::set_target_in_velocitymode(int id, double value){
+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 = value - target_angle_previous[id];
@@ -225,8 +236,18 @@ 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) ){
+        //do a pd control for velocity moves that incorporates position errors:
+        humotion::Timestamp data_ts = get_ts_position(id).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(id).get_interpolated_value(data_ts);
+        double error_d = (position_error - last_position_error[id]) / (((double) rate)*1000.0);
+        last_position_error[id] = position_error;
+        //finally do a PD loop to get the target velocity
+        double target_velocity = PID_P[id] * position_error + PID_D[id]*error_d + speed;
+
         //if (id == ICUB_ID_NECK_PAN) speed = -speed;
-        ivel->velocityMove(id, 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],value,distance,speed);
     }
 
diff --git a/src/server/reflexxes_motion_generator.cpp b/src/server/reflexxes_motion_generator.cpp
index 4e4c61b..d580726 100644
--- a/src/server/reflexxes_motion_generator.cpp
+++ b/src/server/reflexxes_motion_generator.cpp
@@ -83,7 +83,7 @@ void ReflexxesMotionGenerator::reflexxes_set_input(int dof, float target,
     printf("HTS: diff = %f ms, pos=%f --> %f\n", time_diff*1000.0,current_position, position_now);
 
 
-    reflexxes_position_input->CurrentPositionVector->VecData[dof]  = position_now;
+    //reflexxes_position_input->CurrentPositionVector->VecData[dof]  = position_now;
     //reflexxes_position_input->CurrentVelocityVector->VecData[dof]  = current_speed;
 
     // safety: libreflexxes does not like zero accellerations...
@@ -108,7 +108,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->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];
     }
