commit 6c028e110303476b231f4114aab73c860d0a4050
Author: Simon Schulz <sschulz@cit-ec.uni-bielefeld.de>
Date:   Wed Feb 10 16:58:42 2016 +0100

    working partially, eye compensation still buggy

diff --git a/examples/yarp_icub/include/icub_jointinterface.h b/examples/yarp_icub/include/icub_jointinterface.h
index e8d66b6..3f21c4a 100644
--- a/examples/yarp_icub/include/icub_jointinterface.h
+++ b/examples/yarp_icub/include/icub_jointinterface.h
@@ -25,7 +25,7 @@ public:
     iCubJointInterface(std::string scope);
     ~iCubJointInterface();
 
-    void store_incoming_position(int humotion_id, double value, double timestamp);
+    void store_incoming_position(int humotion_id, double position, double timestamp);
     void run();
 
     int convert_icub_jointid_to_humotion(int id);
diff --git a/examples/yarp_icub/src/icub_data_receiver.cpp b/examples/yarp_icub/src/icub_data_receiver.cpp
index f5e4f9e..8aea9bc 100644
--- a/examples/yarp_icub/src/icub_data_receiver.cpp
+++ b/examples/yarp_icub/src/icub_data_receiver.cpp
@@ -57,7 +57,8 @@ void iCubDataReceiver::run() {
 }
 
 void iCubDataReceiver::store_incoming_position(int icub_id, double value, double timestamp) {
-    cout << "iCubDataReceiver::store_incoming_position(" << icub_id << ", " << value << "..)\n";
+    cout << "iCubDataReceiver::store_incoming_position(icubid=" << icub_id << ", " << value << "..)\n";
+
     // store joint position in humotion backend
     if ((icub_id == iCubJointInterface::ICUB_ID_EYES_PAN) ||
             (icub_id == iCubJointInterface::ICUB_ID_EYES_VERGENCE)) {
@@ -67,7 +68,7 @@ void iCubDataReceiver::store_incoming_position(int icub_id, double value, double
         if (icub_id == iCubJointInterface::ICUB_ID_EYES_PAN) {
             target_eye_pan_ = value;
         } else {
-            target_eye_vergence_ = value;
+            target_eye_vergence_ = -value;
         }
 
         float left  = target_eye_pan_ + target_eye_vergence_/2.0;
@@ -82,6 +83,11 @@ void iCubDataReceiver::store_incoming_position(int icub_id, double value, double
         //icub_jointinterface->store_incoming_position(ID_EYES_RIGHT_LID_UPPER,
         //                                             lid_angle, timestamp);
     } else {
+        if (icub_id == iCubJointInterface::ID_NECK_PAN) {
+            // icub uses an inverted neck pan specification
+            value = -value;
+        }
+
         // known configured mapping between joint ids
         int humotion_id = icub_jointinterface_->convert_icub_jointid_to_humotion(icub_id);
         icub_jointinterface_->store_incoming_position(humotion_id, value, timestamp);
diff --git a/examples/yarp_icub/src/icub_jointinterface.cpp b/examples/yarp_icub/src/icub_jointinterface.cpp
index cb75bb0..968f6a1 100644
--- a/examples/yarp_icub/src/icub_jointinterface.cpp
+++ b/examples/yarp_icub/src/icub_jointinterface.cpp
@@ -165,9 +165,13 @@ void iCubJointInterface::publish_target(int humotion_id, float position, float v
         float target_velocity_left  = get_target_velocity(JointInterface::ID_EYES_LEFT_LR);
         float target_velocity_right = get_target_velocity(JointInterface::ID_EYES_RIGHT_LR);
 
+
         // calculate target angles
-        float target_position_pan      = (target_position_left + target_position_right) / 2;
-        float target_position_vergence = (target_position_left - target_position_right);
+        float target_position_pan      = (target_position_right + target_position_left) / 2;
+        float target_position_vergence = (target_position_right - target_position_left);
+
+        cout << "LR " << target_position_left << " " << target_position_right <<
+                " PAN " << target_position_pan << " VERGENCE " << target_position_vergence << "\n";
 
         // calculate target velocities
         // for now just use the same velocity for pan and vergence
@@ -192,6 +196,11 @@ void iCubJointInterface::publish_target(int humotion_id, float position, float v
 //! \param float value of position
 void iCubJointInterface::store_icub_joint_target(int icub_id, float position, float velocity) {
     printf("> set icub joint %d -> p = %f, v = %f\n",icub_id,position,velocity);
+    if (icub_id == ICUB_ID_NECK_PAN) {
+        // icub uses an inverted neck pan specification
+        position = -position;
+        velocity = -velocity;
+    }
     target_angle_[icub_id] = position;
     target_velocity_[icub_id] = velocity;
 }
@@ -217,7 +226,7 @@ void iCubJointInterface::set_target_in_velocitymode(int icub_id) {
     assert(!POSITION_CONTROL);
 
     // fetch humotion id from icub joint id
-    int humotion_id = convert_humotion_jointid_to_icub(icub_id);
+    int humotion_id = convert_icub_jointid_to_humotion(icub_id);
 
     // fetch the target velocity
     float target_velocity = target_velocity_[icub_id];
@@ -228,14 +237,14 @@ void iCubJointInterface::set_target_in_velocitymode(int icub_id) {
 
     //execute:
     //ivel->velocityMove(id, speed);
-    if ((icub_id != ICUB_ID_NECK_PAN)  &&
+    /*if ((icub_id != ICUB_ID_NECK_PAN)  &&
         (icub_id != ICUB_ID_EYES_BOTH_UD) &&
         (icub_id != ICUB_ID_NECK_TILT) &&
         (icub_id != ICUB_ID_EYES_BOTH_UD) &&
         (icub_id != ICUB_ID_NECK_TILT)) {
         // limit to some joints for debugging...
         return;
-    }
+    }*/
 
     // we now add a pd control loop for velocity moves in order to handle position errors
     // TODO: add position interpolation into future. this requires to enable the velocity
@@ -315,8 +324,11 @@ void iCubJointInterface::execute_motion(){
 //! \param id of joint
 //! \param float value of position
 //! \param double timestamp
-void iCubJointInterface::store_incoming_position(int humotion_id, double value, double timestamp){
-    JointInterface::store_incoming_position(humotion_id, value, timestamp);
+void iCubJointInterface::store_incoming_position(int humotion_id, double position, double timestamp){
+    cout << "iCubJointInterface::store_incoming_position(humotionid=" << humotion_id <<
+            ", " << position << ", ...)\n";
+
+    JointInterface::store_incoming_position(humotion_id, position, timestamp);
 }
 /*
     //store joint based on id:
