commit 1f748ce774073e0f9d36de935d134a6374ef28f2
Author: Simon Schulz <sschulz@cit-ec.uni-bielefeld.de>
Date:   Thu Feb 11 11:07:20 2016 +0100

    adding speed from icub

diff --git a/examples/yarp_icub/include/icub_jointinterface.h b/examples/yarp_icub/include/icub_jointinterface.h
index 047f89f..8eabcdf 100644
--- a/examples/yarp_icub/include/icub_jointinterface.h
+++ b/examples/yarp_icub/include/icub_jointinterface.h
@@ -74,6 +74,7 @@ private:
     // yarp views
     yarp::dev::IVelocityControl *yarp_ivel_;
     yarp::dev::IPositionControl *yarp_ipos_;
+
     //yarp::dev::IEncodersTimed *yarp_iencs_;
     yarp::dev::IControlLimits *yarp_ilimits_;
     yarp::dev::IAmplifierControl *yarp_amp_;
@@ -105,24 +106,24 @@ private:
     void init_joints();
 
     std::string scope;
-    yarp::sig::Vector positions;
-    yarp::sig::Vector velocities;
+    //yarp::sig::Vector positions;
+    //yarp::sig::Vector velocities;
 
 
     void store_min_max(yarp::dev::IControlLimits *ilimits, int id, int e);
 
-    float last_pos_eye_vergence;
-    float last_pos_eye_pan;
-    float last_vel_eye_vergence;
-    float last_vel_eye_pan;
+   //float last_pos_eye_vergence;
+    //float last_pos_eye_pan;
+   // float last_vel_eye_vergence;
+    //float last_vel_eye_pan;
 
     void store_joint(int id, float value);
     void set_target_in_positionmode(int id);
     void set_target_in_velocitymode(int id);
 
 
-    int convert_enum_to_motorid(int e);
-    int convert_motorid_to_enum(int id);
+    //int convert_enum_to_motorid(int e);
+    //int convert_motorid_to_enum(int id);
 
     iCubFaceInterface *face_interface_;
 
diff --git a/examples/yarp_icub/src/icub_data_receiver.cpp b/examples/yarp_icub/src/icub_data_receiver.cpp
index 278e76e..1cdbe9b 100644
--- a/examples/yarp_icub/src/icub_data_receiver.cpp
+++ b/examples/yarp_icub/src/icub_data_receiver.cpp
@@ -1,13 +1,10 @@
 #include "icub_data_receiver.h"
 #include <humotion/server/joint_interface.h>
 #include <yarp/os/Property.h>
-#include <boost/format.hpp>
-//using namespace yarp::dev;
-//using namespace yarp::sig;
-//using namespace yarp::os;
+//#include <boost/format.hpp>
 using std::cout;
+using std::cerr;
 using std::string;
-
 using humotion::server::JointInterface;
 using yarp::dev::IEncodersTimed;
 using yarp::sig::Vector;
@@ -24,7 +21,7 @@ iCubDataReceiver::iCubDataReceiver(int period, iCubJointInterface *icub_jointint
     yarp::dev::PolyDriver *poly_driver = icub_jointinterface->get_yarp_polydriver();
     bool success = poly_driver->view(iencs_);
     if (!success) {
-        cout << "ERROR: polydriver failed to init iencs view\n";
+        cerr << "ERROR: polydriver failed to init iencs view\n";
         exit(EXIT_FAILURE);
     }
 
@@ -43,12 +40,10 @@ bool iCubDataReceiver::threadInit() {
 void iCubDataReceiver::threadRelease() {
 }
 
-yarp::sig::Vector iCubDataReceiver::calculate_velocities(yarp::sig::Vector positions,
-                                                         yarp::sig::Vector timestamps) {
-    yarp::sig::Vector velocities;
+Vector iCubDataReceiver::calculate_velocities(Vector positions, Vector timestamps) {
+    Vector velocities;
     velocities.resize(positions.size());
 
-    cout << "\n";
     if (previous_positions_.size() == 0){
         // first run, no valid old position available, return zero velocities
         // by setting all all elements to zero
@@ -60,10 +55,8 @@ yarp::sig::Vector iCubDataReceiver::calculate_velocities(yarp::sig::Vector posit
             float timediff = timestamps[i] - previous_timestamps_[i];
             // calc speed:
             velocities[i] = diff / timediff;
-            cout << " [" << i << "]=" <<  boost::format("%6.3f") % velocities[i];
         }
     }
-    cout << " VEL\n";
 
     previous_positions_ = positions;
     previous_timestamps_ = timestamps;
diff --git a/examples/yarp_icub/src/icub_jointinterface.cpp b/examples/yarp_icub/src/icub_jointinterface.cpp
index cd7f122..d8e7443 100644
--- a/examples/yarp_icub/src/icub_jointinterface.cpp
+++ b/examples/yarp_icub/src/icub_jointinterface.cpp
@@ -298,8 +298,15 @@ void iCubJointInterface::execute_motion(){
     //printf("> TARGET PAN = %3.2f\n",target_angle[ICUB_ID_NECK_PAN]);
 
 
-    //eyelids: unfortuantely the icub has only 1dof for eyelids, so we use only one dof here:
-    face_interface_->set_eyelid_angle(target_angle_[ICUB_ID_EYES_RIGHT_LID_UPPER]);
+    // eyelids: unfortuantely the icub has only 1dof for eyelids
+    // therefore we can only use an opening value
+    float opening_left  = target_angle_[ICUB_ID_EYES_LEFT_LID_UPPER]
+                          - target_angle_[ICUB_ID_EYES_LEFT_LID_LOWER];
+    float opening_right = target_angle_[ICUB_ID_EYES_RIGHT_LID_UPPER]
+                          - target_angle_[ICUB_ID_EYES_RIGHT_LID_LOWER];
+    float opening = (opening_left + opening_right) / 2.0;
+    // send it to icub face if
+    face_interface_->set_eyelid_angle(opening);
 
     //eyebrows are set using a special command as well:
     face_interface_->set_eyebrow_angle(ICUB_ID_EYES_LEFT_BROW, target_angle_);
