commit 214449152b62056d4be3e2d5b851fd7205ae35da
Author: Simon Schulz <sschulz@cit-ec.uni-bielefeld.de>
Date:   Thu Feb 4 16:41:22 2016 +0100

    testing vel control

diff --git a/CMakeLists.txt b/CMakeLists.txt
index ce80a1c..e520867 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -10,20 +10,21 @@ set(ROS_BUILD_TYPE Debug)
 find_package(catkin REQUIRED COMPONENTS roscpp std_msgs sensor_msgs message_generation genmsg)
 IF (NOT catkin_FOUND)
   MESSAGE(FATAL_ERROR "Error: could not find ROS middleware!")
-ENDIF (catkin_FOUND)
+ENDIF (NOT catkin_FOUND)
 
 INCLUDE(FindPkgConfig)
 
 ##libreflexxes
-IF (libreflexxes_DIR)
-  MESSAGE("using libreflexxes_DIR as override ('${libreflexxes_DIR}')")
-  SET(REFLEXXES_PREFIX ${libreflexxes_DIR})
-ELSE ()  
+#IF (libreflexxes_DIR)
+#  MESSAGE("using libreflexxes_DIR as override ('${libreflexxes_DIR}')")
+#  SET(REFLEXXES_PREFIX ${libreflexxes_DIR})
+#  SET(REFLEXXES_LIBRARY "ReflexxesTypeII")
+#ELSE ()
   PKG_CHECK_MODULES(REFLEXXES REQUIRED libReflexxesTypeII>=1.2.3)
   IF (NOT REFLEXXES_FOUND)
     message(FATAL_ERROR "Error: could not find lib libReflexxesTypeII")
   ENDIF ()
-ENDIF ()
+#ENDIF ()
 
 
 SET(REFLEXXES_LIBRARY_DIRS "${REFLEXXES_PREFIX}/lib")
@@ -80,7 +81,7 @@ catkin_package(
 
 ## Specify additional locations of header files
 ## Your package locations should be listed before other locations
-include_directories (BEFORE ${Boost_INCLUDE_DIRS} ${REFLEXXES_INCLUDE_DIRS})
+include_directories(BEFORE ${Boost_INCLUDE_DIRS} ${REFLEXXES_INCLUDE_DIRS})
 include_directories(BEFORE include)
 include_directories(BEFORE include/humotion)
 #make sure to use ros messages from current build
diff --git a/examples/yarp_icub/CMakeLists.txt.user b/examples/yarp_icub/CMakeLists.txt.user
index 4649f6e..be1bbb0 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, 2015-03-12T13:07:35. -->
+<!-- Written by QtCreator 3.0.1, 2016-02-04T09:55:06. -->
 <qtcreator>
  <data>
   <variable>ProjectExplorer.Project.ActiveTarget</variable>
@@ -53,13 +53,14 @@
   <valuemap type="QVariantMap">
    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">Desktop</value>
    <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName">Desktop</value>
-   <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">{3152ed58-41d1-426c-b17f-dbe12171436a}</value>
+   <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">{0ff36a7d-682d-4601-a9c1-275f77eefc70}</value>
    <value type="int" key="ProjectExplorer.Target.ActiveBuildConfiguration">0</value>
    <value type="int" key="ProjectExplorer.Target.ActiveDeployConfiguration">0</value>
    <value type="int" key="ProjectExplorer.Target.ActiveRunConfiguration">0</value>
    <valuemap type="QVariantMap" key="ProjectExplorer.Target.BuildConfiguration.0">
     <value type="bool" key="CMakeProjectManager.CMakeBuildConfiguration.UseNinja">false</value>
-    <value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/homes/sschulz/src/flobidev.core/humotion/examples/yarp_icub/qbuild</value>
+    <value type="QString" key="CMakeProjectManager.CMakeBuildConfiguration.UserArguments">-DCMAKE_INSTALL_PREFIX=/vol/icub/sschulz/releases/roman16-humotion-nightly/</value>
+    <value type="QString" key="ProjectExplorer.BuildConfiguration.BuildDirectory">/homes/sschulz/src/flobi/humotion/examples/yarp_icub/build</value>
     <valuemap type="QVariantMap" key="ProjectExplorer.BuildConfiguration.BuildStepList.0">
      <valuemap type="QVariantMap" key="ProjectExplorer.BuildStepList.Step.0">
       <value type="QString" key="CMakeProjectManager.MakeStep.AdditionalArguments"></value>
@@ -153,14 +154,14 @@
     <value type="QString" key="CMakeProjectManager.CMakeRunConfiguration.Arguments"></value>
     <value type="bool" key="CMakeProjectManager.CMakeRunConfiguration.UseTerminal">false</value>
     <value type="QString" key="CMakeProjectManager.CMakeRunConfiguration.UserWorkingDirectory"></value>
-    <value type="int" key="PE.EnvironmentAspect.Base">-1</value>
+    <value type="int" key="PE.EnvironmentAspect.Base">2</value>
     <valuelist type="QVariantList" key="PE.EnvironmentAspect.Changes"/>
     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DefaultDisplayName">icub_humotion_server</value>
     <value type="QString" key="ProjectExplorer.ProjectConfiguration.DisplayName"></value>
     <value type="QString" key="ProjectExplorer.ProjectConfiguration.Id">CMakeProjectManager.CMakeRunConfiguration.icub_humotion_server</value>
     <value type="uint" key="RunConfiguration.QmlDebugServerPort">3768</value>
-    <value type="bool" key="RunConfiguration.UseCppDebugger">false</value>
-    <value type="bool" key="RunConfiguration.UseCppDebuggerAuto">true</value>
+    <value type="bool" key="RunConfiguration.UseCppDebugger">true</value>
+    <value type="bool" key="RunConfiguration.UseCppDebuggerAuto">false</value>
     <value type="bool" key="RunConfiguration.UseMultiProcess">false</value>
     <value type="bool" key="RunConfiguration.UseQmlDebugger">false</value>
     <value type="bool" key="RunConfiguration.UseQmlDebuggerAuto">true</value>
@@ -174,7 +175,7 @@
  </data>
  <data>
   <variable>ProjectExplorer.Project.Updater.EnvironmentId</variable>
-  <value type="QByteArray">{ccede451-c1b6-4b92-a13d-14bd6a65dd35}</value>
+  <value type="QByteArray">{44110186-3313-405d-bf42-c546b33c1fcb}</value>
  </data>
  <data>
   <variable>ProjectExplorer.Project.Updater.FileVersion</variable>
diff --git a/include/humotion/server/joint_interface.h b/include/humotion/server/joint_interface.h
index 461143e..1e74697 100644
--- a/include/humotion/server/joint_interface.h
+++ b/include/humotion/server/joint_interface.h
@@ -50,7 +50,7 @@ public:
     JointInterface();
     ~JointInterface();
 
-    void set_target_position(int joint_id, float position);
+    void set_target_position(int joint_id, float position, float speed);
     float get_target_position(int joint_id);
     virtual void publish_target_position(int joint_id) = 0;
     virtual void execute_motion() = 0;
@@ -114,6 +114,7 @@ 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];
 
 
 
diff --git a/include/humotion/server/reflexxes_motion_generator.h b/include/humotion/server/reflexxes_motion_generator.h
index aa9883e..525a48a 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 max_speed, float max_accel);
+    void reflexxes_set_input(int dof, float target, float position, 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 6aa9799..1572199 100644
--- a/src/server/eye_motion_generator.cpp
+++ b/src/server/eye_motion_generator.cpp
@@ -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, max_speed, max_accel);
+    reflexxes_set_input(dof, target, now, max_speed, max_accel);
 }
 
 //! calculate joint targets
@@ -100,14 +100,23 @@ void EyeMotionGenerator::calculate_targets(){
     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";
 
     //call reflexxes to handle profile calculation:
     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]);
-    joint_interface->set_target_position(JointInterface::ID_EYES_RIGHT_LR, reflexxes_position_output->NewPositionVector->VecData[1]);
-    joint_interface->set_target_position(JointInterface::ID_EYES_BOTH_UD,  reflexxes_position_output->NewPositionVector->VecData[2]);
+    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]);
+
+    cout << reflexxes_position_output->NewPositionVector->VecData[2] << " " << reflexxes_position_output->NewVelocityVector->VecData[2] ;
 }
 
 
diff --git a/src/server/eyebrow_motion_generator.cpp b/src/server/eyebrow_motion_generator.cpp
index 6a04b48..dcc19ea 100644
--- a/src/server/eyebrow_motion_generator.cpp
+++ b/src/server/eyebrow_motion_generator.cpp
@@ -22,8 +22,8 @@ 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);
-    joint_interface->set_target_position(JointInterface::ID_EYES_RIGHT_BROW, eyebrow_right_target);
+    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);
 }
 
 //! publish targets to motor boards:
diff --git a/src/server/eyelid_motion_generator.cpp b/src/server/eyelid_motion_generator.cpp
index 8c13bf0..e3eba0f 100644
--- a/src/server/eyelid_motion_generator.cpp
+++ b/src/server/eyelid_motion_generator.cpp
@@ -81,10 +81,10 @@ void EyelidMotionGenerator::calculate_targets(){
     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);
-    joint_interface->set_target_position(JointInterface::ID_EYES_LEFT_LID_LOWER,  eyelid_lower_left_target);
-    joint_interface->set_target_position(JointInterface::ID_EYES_RIGHT_LID_UPPER,  eyelid_upper_right_target);
-    joint_interface->set_target_position(JointInterface::ID_EYES_RIGHT_LID_LOWER,  eyelid_lower_right_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);
 
     //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);
+        joint_interface->set_target_position(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);
+        joint_interface->set_target_position(joint_id, value, 0.0);
         break;
     }
 }
diff --git a/src/server/joint_interface.cpp b/src/server/joint_interface.cpp
index dc0ef87..a2f25b1 100644
--- a/src/server/joint_interface.cpp
+++ b/src/server/joint_interface.cpp
@@ -48,11 +48,12 @@ JointInterface::~JointInterface(){
 //! set joint target position
 //! \param joint_id of joint
 //! \param float position
-void JointInterface::set_target_position(int joint_id, float position){
+void JointInterface::set_target_position(int joint_id, float position, float speed){
     assert(joint_id < JOINT_ID_ENUM_SIZE);
 
     //update current value
     joint_target[joint_id] = position;
+    joint_target_speed[joint_id] = speed;
 }
 
 //! fetch target position
diff --git a/src/server/motion_generator.cpp b/src/server/motion_generator.cpp
index fe5f251..b70e0a5 100644
--- a/src/server/motion_generator.cpp
+++ b/src/server/motion_generator.cpp
@@ -49,6 +49,11 @@ MotionGenerator::~MotionGenerator(){
 //! \param joint_id
 //! \return float value of joint position
 float MotionGenerator::get_current_position(int joint_id){
+    /*Timestamp tsl = joint_interface->get_ts_position(joint_id).get_last_timestamp();
+    Timestamp now = Timestamp::now();
+    Timestamp diff=now-tsl;
+    printf("TIME DIFF %fs %fns\n",diff.sec, diff.nsec);*/
+
     return joint_interface->get_ts_position(joint_id).get_newest_value();
 }
 
diff --git a/src/server/mouth_motion_generator.cpp b/src/server/mouth_motion_generator.cpp
index 339bc00..1faecb8 100644
--- a/src/server/mouth_motion_generator.cpp
+++ b/src/server/mouth_motion_generator.cpp
@@ -92,8 +92,8 @@ void MouthMotionGenerator::update_mouth_target(int upper_id, int lower_id){
     }
 
     //tell the joint about the new values:
-    joint_interface->set_target_position(upper_id, unsafe_target_upper);
-    joint_interface->set_target_position(lower_id, unsafe_target_lower);
+    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);
 }
diff --git a/src/server/neck_motion_generator.cpp b/src/server/neck_motion_generator.cpp
index 4928936..61dfbdf 100644
--- a/src/server/neck_motion_generator.cpp
+++ b/src/server/neck_motion_generator.cpp
@@ -151,9 +151,16 @@ 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]);
-    joint_interface->set_target_position(JointInterface::ID_NECK_TILT, reflexxes_position_output->NewPositionVector->VecData[1]);
-    joint_interface->set_target_position(JointInterface::ID_NECK_ROLL, reflexxes_position_output->NewPositionVector->VecData[2]);
+    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_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]);
 }
 
 //! publish targets to motor boards:
@@ -206,9 +213,8 @@ void NeckMotionGenerator::setup_neckmotion(int dof, float target, float now){
     if (max_accel>1000){
         max_accel = 1000;
     }
-
     ///printf("MAX SPEED %4.2f / max accel %4.2f\n",max_speed, max_accel);
 
     //feed reflexxes api with data
-    reflexxes_set_input(dof, target, max_speed, max_accel);
+    reflexxes_set_input(dof, target, now, max_speed, max_accel);
 }
diff --git a/src/server/reflexxes_motion_generator.cpp b/src/server/reflexxes_motion_generator.cpp
index c16bd77..eb2bd20 100644
--- a/src/server/reflexxes_motion_generator.cpp
+++ b/src/server/reflexxes_motion_generator.cpp
@@ -35,7 +35,7 @@ using namespace humotion::server;
 ReflexxesMotionGenerator::ReflexxesMotionGenerator(JointInterface *j, int dof, float t) : MotionGenerator(j){
     dof_count = dof;
 
-    //create Reflexxes API for 3 DOF
+    //create Reflexxes API for <dof> DOF actuator
     reflexxes_api = new ReflexxesAPI(dof, t);
     reflexxes_position_input  = new RMLPositionInputParameters(dof);
     reflexxes_position_output = new RMLPositionOutputParameters(dof);
@@ -54,13 +54,22 @@ 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 max_speed, float max_accel){
+void ReflexxesMotionGenerator::reflexxes_set_input(int dof, float target, float position, float max_speed, float max_accel){
+    assert(dof < dof_count);
+
     //set up reflexxes:
     reflexxes_position_input->TargetPositionVector->VecData[dof]  = target;
     reflexxes_position_input->SelectionVector->VecData[dof]       = true;
     reflexxes_position_input->MaxVelocityVector->VecData[dof]     = max_speed;
-    reflexxes_position_input->MaxAccelerationVector->VecData[dof] = 0.0001 + max_accel;
+    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;
+
+    // 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
@@ -78,7 +87,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];
     }
diff --git a/src/timestamped_list.cpp b/src/timestamped_list.cpp
index a162698..4724178 100644
--- a/src/timestamped_list.cpp
+++ b/src/timestamped_list.cpp
@@ -137,7 +137,7 @@ float TimestampedList::get_interpolated_value(Timestamp target_ts){
     }
 
     //we reached the end, return the last value
-    printf("> warning: found no timestamp bigger than %f in timestamped list...\n", target_ts.to_seconds());
+    printf("> warning: found no timestamp >= than %f in timestamped list...\n", target_ts.to_seconds());
     printf("           this should not happen as images will always be behind\n");
     printf("           the motor data. returning most recent value (ts=%f)\n", previous.timestamp.to_seconds());
 
