commit 730467d3acdee5b101f5c756d5577a4844d30851
Author: Simon Schulz <sschulz@cit-ec.uni-bielefeld.de>
Date:   Tue Feb 9 13:12:54 2016 +0100

    testing delay

diff --git a/include/humotion/server/eye_motion_generator.h b/include/humotion/server/eye_motion_generator.h
index cbc0832..6431d99 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 current_position, float current_speed);
+    void setup_eyemotion(int dof, float target, float current_position, float current_speed, Timestamp timestamp);
 
     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 2a4b85d..ad4fde0 100644
--- a/include/humotion/server/motion_generator.h
+++ b/include/humotion/server/motion_generator.h
@@ -47,6 +47,7 @@ public:
 protected:
     float get_current_position(int joint_id);
     float get_current_speed(int joint_id);
+    humotion::Timestamp get_timestamped_state(int joint_id, float *position, float *velocity);
     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 87e4afd..be505b2 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 current_position, float current_speed);
+    void setup_neckmotion(int dof, float target, float current_position, float current_speed, Timestamp timestamp);
 
 private:
     float get_breath_offset();
diff --git a/include/humotion/server/reflexxes_motion_generator.h b/include/humotion/server/reflexxes_motion_generator.h
index da33839..ee889bb 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 current_position, float current_speed, float max_speed, float max_accel);
+    void reflexxes_set_input(int dof, float target, float current_position, float current_speed, Timestamp timestamp, float max_speed, float max_accel);
     void reflexxes_calculate_profile();
 
     //*****************************************
diff --git a/src/server/eye_motion_generator.cpp b/src/server/eye_motion_generator.cpp
index 98df32b..0b22367 100644
--- a/src/server/eye_motion_generator.cpp
+++ b/src/server/eye_motion_generator.cpp
@@ -49,7 +49,10 @@ EyeMotionGenerator::~EyeMotionGenerator(){
 //! \param dof id of joint
 //! \param target angle
 //! \param current angle
-void EyeMotionGenerator::setup_eyemotion(int dof, float target, float current_position, float current_speed){
+void EyeMotionGenerator::setup_eyemotion(int dof, float target,
+                                         float current_position,
+                                         float current_speed,
+                                         humotion::Timestamp timestamp){
     //get distance to target:
     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)
@@ -60,7 +63,7 @@ void EyeMotionGenerator::setup_eyemotion(int dof, float target, float current_po
     float max_accel = fmin(80000.0, 1526.53*distance_abs + 10245.4);
 
     //feed reflexxes api with data
-    reflexxes_set_input(dof, target, current_position, current_speed, max_speed, max_accel);
+    reflexxes_set_input(dof, target, current_position, current_speed, timestamp, max_speed, max_accel);
 }
 
 //! calculate joint targets
@@ -91,19 +94,29 @@ void EyeMotionGenerator::calculate_targets(){
     eye_pan_r_target = limit_target(JointInterface::ID_EYES_RIGHT_LR, eye_pan_r_target);
     eye_tilt_target = limit_target(JointInterface::ID_EYES_BOTH_UD, eye_tilt_target);
 
-    //fetch current angles:
-    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);
+    //fetch current dataset
+    float eye_pan_l_now, eye_pan_r_now, eye_tilt_now;
+    float eye_pan_l_speed, eye_pan_r_speed, eye_tilt_speed;
+
+    humotion::Timestamp eye_pan_l_ts = get_timestamped_state(
+                JointInterface::ID_EYES_LEFT_LR,
+                &eye_pan_l_now,
+                &eye_pan_l_speed);
+
+    humotion::Timestamp eye_pan_r_ts = get_timestamped_state(
+                JointInterface::ID_EYES_RIGHT_LR,
+                &eye_pan_r_now,
+                &eye_pan_r_speed);
+
+    humotion::Timestamp eye_tilt_ts = get_timestamped_state(
+                JointInterface::ID_EYES_BOTH_UD,
+                &eye_tilt_now,
+                &eye_tilt_speed);
 
     //pass paramaters to reflexxes api:
-    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);
+    setup_eyemotion(0, eye_pan_l_target, eye_pan_l_now, eye_pan_l_speed, eye_pan_l_ts);
+    setup_eyemotion(1, eye_pan_r_target, eye_pan_r_now, eye_pan_r_speed, eye_pan_r_ts);
+    setup_eyemotion(2, eye_tilt_target, eye_tilt_now, eye_tilt_speed,    eye_tilt_ts);
 
     // cout << "EYE MOTION 2 " << eye_tilt_target << " now=" << eye_tilt_now << "\n";
 
diff --git a/src/server/motion_generator.cpp b/src/server/motion_generator.cpp
index a8099b1..f1b3040 100644
--- a/src/server/motion_generator.cpp
+++ b/src/server/motion_generator.cpp
@@ -45,6 +45,19 @@ MotionGenerator::MotionGenerator(JointInterface *j){
 MotionGenerator::~MotionGenerator(){
 }
 
+//! fetch the latest position and velocity and return the timestamp of that dataset
+//! \param joint id
+//! \param pointer to position variable
+//! \param pointer to veolocity variable
+//! \return Timestamp of this dataset
+humotion::Timestamp MotionGenerator::get_timestamped_state(int joint_id,
+                                                           float *position, float *velocity) {
+    humotion::Timestamp stamp = joint_interface->get_ts_position(joint_id).get_last_timestamp();
+    *position = joint_interface->get_ts_position(joint_id).get_interpolated_value(stamp);
+    *velocity = joint_interface->get_ts_speed(joint_id).get_interpolated_value(stamp);
+    return stamp;
+}
+
 //! fetch the latest (=current) speed of a joint
 //! \param joint_id
 //! \return float value of joint speed
diff --git a/src/server/neck_motion_generator.cpp b/src/server/neck_motion_generator.cpp
index 3a0c344..0494e40 100644
--- a/src/server/neck_motion_generator.cpp
+++ b/src/server/neck_motion_generator.cpp
@@ -91,14 +91,21 @@ float NeckMotionGenerator::get_breath_offset(){
 
 //! calculate joint targets
 void NeckMotionGenerator::calculate_targets(){
-    //fetch current angles:
-    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);
+    //fetch current dataset:
+    float neck_pan_now, neck_tilt_now, neck_roll_now;
+    float neck_pan_speed, neck_tilt_speed, neck_roll_speed;
+
+    humotion::Timestamp neck_pan_ts = get_timestamped_state(JointInterface::ID_NECK_PAN,
+                                                            &neck_pan_now,
+                                                            &neck_pan_speed);
+
+    humotion::Timestamp neck_tilt_ts = get_timestamped_state(JointInterface::ID_NECK_TILT,
+                                                            &neck_tilt_now,
+                                                            &neck_tilt_speed);
+
+    humotion::Timestamp neck_roll_ts = get_timestamped_state(JointInterface::ID_NECK_ROLL,
+                                                            &neck_roll_now,
+                                                            &neck_roll_speed);
 
     //reached target?
     float goal_diff   = fabs(get_current_gaze().distance_pt_abs(requested_gaze_state));
@@ -147,9 +154,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, 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);
+    setup_neckmotion(0, neck_pan_target,  neck_pan_now,  neck_pan_speed,  neck_pan_ts);
+    setup_neckmotion(1, neck_tilt_target, neck_tilt_now, neck_tilt_speed, neck_tilt_ts);
+    setup_neckmotion(2, neck_roll_target, neck_roll_now, neck_roll_speed, neck_roll_ts);
 
     //call reflexxes to handle profile calculation:
     reflexxes_calculate_profile();
@@ -167,7 +174,7 @@ void NeckMotionGenerator::calculate_targets(){
                                          reflexxes_position_output->NewPositionVector->VecData[2],
                                          reflexxes_position_output->NewVelocityVector->VecData[2]);
 
-    printf("\n%f %f %f %f %f %f DBG\n",
+    printf("\n%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(),
@@ -191,7 +198,8 @@ void NeckMotionGenerator::publish_targets(){
 //! \param dof id of joint
 //! \param target angle
 //! \param current angle
-void NeckMotionGenerator::setup_neckmotion(int dof, float target, float current_position, float current_speed){
+void NeckMotionGenerator::setup_neckmotion(int dof, float target, float current_position,
+                                           float current_speed, humotion::Timestamp timestamp){
     //get distance to target:
     float distance_abs = fabs(target - current_position);
 
@@ -228,5 +236,5 @@ void NeckMotionGenerator::setup_neckmotion(int dof, float target, float current_
     ///printf("MAX SPEED %4.2f / max accel %4.2f\n",max_speed, max_accel);
 
     //feed reflexxes api with data
-    reflexxes_set_input(dof, target, current_position, current_speed, max_speed, max_accel);
+    reflexxes_set_input(dof, target, current_position, current_speed, timestamp, max_speed, max_accel);
 }
diff --git a/src/server/reflexxes_motion_generator.cpp b/src/server/reflexxes_motion_generator.cpp
index 48e0623..1a146b1 100644
--- a/src/server/reflexxes_motion_generator.cpp
+++ b/src/server/reflexxes_motion_generator.cpp
@@ -54,7 +54,10 @@ 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 current_position, float current_speed, float max_speed, float max_accel){
+void ReflexxesMotionGenerator::reflexxes_set_input(int dof, float target,
+                                                   float current_position, float current_speed,
+                                                   humotion::Timestamp timestamp,
+                                                   float max_speed, float max_accel){
     assert(dof < dof_count);
 
     //set up reflexxes:
@@ -63,7 +66,14 @@ 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?)
+
     // feed back current pos & velocity
+    // as we have to deal with some latency we will forecast the current
+    // position using the old speed, position and the latency:
+    float diff = humotion::Timestamp::now().to_seconds() - timestamp.to_seconds();
+    printf("HTS: diff = %f ms\n", diff*1000.0);
+
+
     //reflexxes_position_input->CurrentPositionVector->VecData[dof]  = current_position;
     //reflexxes_position_input->CurrentVelocityVector->VecData[dof]  = current_speed;
 
