commit dbf664830a65d1c08def8e273ac878b45b817b4a
Author: Florian Lier <flier@techfak.uni-bielefeld.de>
Date:   Wed Jun 8 17:08:14 2016 +0200

    fixed not moving bug

diff --git a/include/humotion/server/controller.h b/include/humotion/server/controller.h
index 1c15ca7..4ea4f63 100644
--- a/include/humotion/server/controller.h
+++ b/include/humotion/server/controller.h
@@ -28,6 +28,7 @@
 #ifndef INCLUDE_HUMOTION_SERVER_CONTROLLER_H_
 #define INCLUDE_HUMOTION_SERVER_CONTROLLER_H_
 
+#include <string>
 #include <vector>
 
 #include "humotion/gaze_state.h"
diff --git a/src/server/controller.cpp b/src/server/controller.cpp
index d1a8781..f2fdce0 100644
--- a/src/server/controller.cpp
+++ b/src/server/controller.cpp
@@ -139,8 +139,8 @@ humotion::GazeState Controller::relative_gaze_to_absolute_gaze(humotion::GazeSta
     // 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());
@@ -152,8 +152,11 @@ humotion::GazeState Controller::relative_gaze_to_absolute_gaze(humotion::GazeSta
         // therefore we will use the last known targets (see below)
         // in case we did not see this timestamp before, show a warning:
         if (last_known_absolute_timestamp_ != relative_target_timestamp) {
-            printf("> WARNING: restored/guessed absolute target for unknown timestamp %f "
-                   "[this should only happen during startup]\n", relative_target_timestamp.to_seconds());
+            printf("> WARNING: restored/guessed absolute target for unknown timestamp %f (tsmap = [%f - %f])"
+                   "[this should only happen during startup]\n", relative_target_timestamp.to_seconds(),
+                   history_begin.to_seconds(),
+                   history_end.to_seconds()
+                   );
             last_known_absolute_target_pan_ = 0.0;
             last_known_absolute_target_tilt_ = 0.0;
             last_known_absolute_target_roll_ = 0.0;
@@ -199,9 +202,18 @@ humotion::GazeState Controller::relative_gaze_to_absolute_gaze(humotion::GazeSta
     // printf("pan  now = %4.1f, rel=%4.1f ===> %4.2f\n", pan, relative.pan, absolute_gaze.pan);
     // printf("tilt now = %4.1f, rel=%4.1f ===> %4.2f\n", tilt, relative.tilt, absolute_gaze.tilt);
 
+    // update the timestamp. this gaze target is now absolute, set appropriate timestamp
+    //printf("%f %f %f 333\n", absolute_gaze.pan, absolute_gaze.tilt, absolute_gaze.roll);
+    //absolute_gaze.tilt = 0.0;
+    //absolute_gaze.roll = 0.0;
+
+    //absolute_gaze.roll = absolute_gaze.pan; //0.012;
+    absolute_gaze.timestamp = Timestamp::now();
+
     // store debug data:
     // this is the position we had at the ts of the relative target
-    store_debug_data("controller/pan_position_at_relative_ts", last_known_absolute_target_pan_);
+    store_debug_data("controller/pan_neck_position_at_relative_ts", neck_pan);
+    store_debug_data("controller/pan_overall_position_at_relative_ts", last_known_absolute_target_pan_);
     // this is the relative movement that was requested
     store_debug_data("controller/pan_target_relative", relative.pan);
     // this is the calculated overall target
diff --git a/src/server/joint_interface.cpp b/src/server/joint_interface.cpp
index f13348b..4d5f85a 100644
--- a/src/server/joint_interface.cpp
+++ b/src/server/joint_interface.cpp
@@ -77,8 +77,8 @@ void JointInterface::store_incoming_position(int joint_id, float position, Times
     // following atomic instructions:
     mutex::scoped_lock sl(joint_ts_position_map_access_mutex_);
 
-    // printf("> humotion: incoming joint position for joint id 0x%02X "
-    // "= %4.2f (ts=%.2f)\n",joint_id,position,timestamp.to_seconds());
+    //printf("> humotion: incoming joint position for joint id 0x%02X "
+    //"= %4.2f (ts=%.2f)\n",joint_id,position,timestamp.to_seconds());
     joint_ts_position_map_[joint_id].insert(timestamp, position);
 
     incoming_position_count_++;
diff --git a/src/server/neck_motion_generator.cpp b/src/server/neck_motion_generator.cpp
index 64ce8f9..256211a 100644
--- a/src/server/neck_motion_generator.cpp
+++ b/src/server/neck_motion_generator.cpp
@@ -141,17 +141,22 @@ void NeckMotionGenerator::calculate_targets() {
     // get targets: this is the sum of stored neck target and up-to-date offset:
     float neck_pan_target  = requested_neck_state_.pan  + requested_gaze_state_.pan_offset;
     float neck_tilt_target = requested_neck_state_.tilt + requested_gaze_state_.tilt_offset;
+
     // roll is always equal to requested gaze (not neck) state
     float neck_roll_target = requested_gaze_state_.roll + requested_gaze_state_.roll_offset;
 
     // add breath wave to tilt
     neck_tilt_target += get_breath_offset();
 
+    //neck_roll_target = 0.0;
+    //printf("%f %f %f %f ROLL\n", neck_pan_target, neck_roll_target, neck_roll_now, neck_roll_speed);
+
     // pass parameters to reflexxes api
     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();
 
@@ -168,10 +173,12 @@ void NeckMotionGenerator::calculate_targets() {
                                 reflexxes_position_output->NewPositionVector->VecData[2],
                                 reflexxes_position_output->NewVelocityVector->VecData[2]);
 
+    //printf("%f %f %f RRR\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(),
+            joint_interface_->get_ts_speed(JointInterface::ID_NECK_PAN).get_newest_value(),
             reflexxes_position_output->NewVelocityVector->VecData[0]
             );*/
 
@@ -216,6 +223,7 @@ void NeckMotionGenerator::setup_neckmotion(int dof, float target, float current_
     // get distance to target
     float distance_abs = fabs(target - current_position);
 
+
     // get max speed: according to the equation Hmax from [guitton87] there is a linear relation
     // between distance_abs and v_max_head:
     // v_max = 4.39 * d_total + 106.0 (in degrees)
@@ -226,6 +234,7 @@ void NeckMotionGenerator::setup_neckmotion(int dof, float target, float current_
     max_velocity = fmin(max_velocity, config->limit_velocity_neck);
 
     // max accel: assuming linear acceleration we have:
+
     /* v ^  _
     *   |  / \
     *   | /   \
@@ -239,7 +248,7 @@ void NeckMotionGenerator::setup_neckmotion(int dof, float target, float current_
     // d_total = a * 2 * d_total / (v_max^2)
     // and therefore
     // a = v_max^2 / d_total
-    float max_accel = 0.0;
+    float max_accel = 1.0;
 
     if (distance_abs > 0.0) {
         max_accel = pow(max_velocity, 2) / distance_abs;
@@ -250,6 +259,7 @@ void NeckMotionGenerator::setup_neckmotion(int dof, float target, float current_
     max_accel = fmin(max_accel, config->limit_acceleration_neck);
 
     // printf("MAX SPEED %4.2f / max accel %4.2f\n",max_speed, max_accel);
+    // printf("%f %f %f ", distance_abs, max_velocity, max_accel);
 
     // feed reflexxes api with data
     reflexxes_set_input(dof, target, current_position, current_velocity,
diff --git a/src/server/reflexxes_motion_generator.cpp b/src/server/reflexxes_motion_generator.cpp
index 1a23a78..49e3312 100644
--- a/src/server/reflexxes_motion_generator.cpp
+++ b/src/server/reflexxes_motion_generator.cpp
@@ -42,6 +42,7 @@ ReflexxesMotionGenerator::ReflexxesMotionGenerator(JointInterface *j, Config *c,
 
     // synchronize phase
     reflexxes_motion_flags.SynchronizationBehavior =
+            //RMLPositionFlags::NO_SYNCHRONIZATION;
             RMLPositionFlags::PHASE_SYNCHRONIZATION_IF_POSSIBLE;
 }
 
diff --git a/src/timestamped_list.cpp b/src/timestamped_list.cpp
index a20b282..1384eaf 100644
--- a/src/timestamped_list.cpp
+++ b/src/timestamped_list.cpp
@@ -34,7 +34,7 @@ using humotion::TimestampedList;
 
 TimestampedList::TimestampedList(unsigned int s) {
     // initialize the list to its desired size:
-    TimestampedFloat now;
+    TimestampedFloat now(Timestamp::now(), 0.0);
     tsf_list_.resize(s, now);
 }
 
