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

    testing vel control and fixed icub eye bug

diff --git a/examples/yarp_icub/src/icub_data_receiver.cpp b/examples/yarp_icub/src/icub_data_receiver.cpp
index 6efe5e1..628c737 100644
--- a/examples/yarp_icub/src/icub_data_receiver.cpp
+++ b/examples/yarp_icub/src/icub_data_receiver.cpp
@@ -5,13 +5,14 @@ using namespace yarp::sig;
 using namespace yarp::os;
 using namespace std;
 
-iCubDataReceiver::iCubDataReceiver(int period, IEncoders *_iencs, iCubJointInterface *_icub_jointinterface):RateThread(period){
+iCubDataReceiver::iCubDataReceiver(int period, IEncodersTimed *_iencs, iCubJointInterface *_icub_jointinterface):RateThread(period){
     iencs = _iencs;
     icub_jointinterface = _icub_jointinterface;
     int joints;
     iencs->getAxes(&joints);
     positions.resize(joints);
     velocities.resize(joints);
+    timestamps.resize(joints);
 }
 
 bool iCubDataReceiver::threadInit(){ return true; }
@@ -24,18 +25,20 @@ double iCubDataReceiver::get_timestamp_ms(){
 }
 
 void iCubDataReceiver::run(){
+
     //grab pos+vel data:
-    iencs->getEncoders(positions.data());
+    iencs->getEncodersTimed(positions.data(), timestamps.data());
     iencs->getEncoderSpeeds(velocities.data());
-    double timestamp = get_timestamp_ms();
+
+    //double timestamp = get_timestamp_ms();
 
     //publish data to humotion
     for(int i=0; i<positions.size(); i++){
-        icub_jointinterface->fetch_position(i, positions[i], timestamp);
-        icub_jointinterface->fetch_speed(i, velocities[i], timestamp);
+        icub_jointinterface->fetch_position(i, positions[i], timestamps[i]);
+        icub_jointinterface->fetch_speed(i, velocities[i], timestamps[i]);
     }
 
     //tell humotion to update lid angle (hack)
-    icub_jointinterface->fetch_position(100, 0.0, timestamp);
+    icub_jointinterface->fetch_position(100, 0.0, get_timestamp_ms());
     //fixme: use real id
 }
diff --git a/examples/yarp_icub/src/icub_jointinterface.cpp b/examples/yarp_icub/src/icub_jointinterface.cpp
index 105399f..09cfee8 100644
--- a/examples/yarp_icub/src/icub_jointinterface.cpp
+++ b/examples/yarp_icub/src/icub_jointinterface.cpp
@@ -16,7 +16,7 @@ http://wiki.icub.org/wiki/Motor_control
 */
 
 //WARNING: DO NOT CHANGE THIS; VELOCITYMODE IS NOT YET IMPLEMENTED
-#define POSITION_CONTROL 1
+#define POSITION_CONTROL 0
 
 
 //! constructor
@@ -45,8 +45,8 @@ iCubJointInterface::iCubJointInterface(string _scope) : humotion::server::JointI
     enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_NECK_ROLL,   ID_NECK_ROLL));
 
     //EYES
-    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_LEFT_LR,   ID_EYES_LEFT_LR));
-    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_RIGHT_LR,   ID_EYES_RIGHT_LR));
+    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_PAN,   ID_EYES_LEFT_LR));
+    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_VERGENCE,   ID_EYES_RIGHT_LR));
     enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_BOTH_UD,   ID_EYES_BOTH_UD));
 
     //EYELIDS
@@ -69,8 +69,11 @@ iCubJointInterface::iCubJointInterface(string _scope) : humotion::server::JointI
     dd.view(ipos);
     dd.view(ivel);
     dd.view(ilimits);
+    dd.view(pid);
+    dd.view(amp);
 
-    if ( (!iencs) || (!ipos) || (!ilimits) || (!ivel)){
+
+    if ( (!iencs) || (!ipos) || (!ilimits) || (!ivel) || (!amp) || (!pid)){
         printf("> ERROR: failed to open icub views\n");
         exit(EXIT_FAILURE);
     }
@@ -92,7 +95,7 @@ iCubJointInterface::iCubJointInterface(string _scope) : humotion::server::JointI
         ipos->setPositionMode();
     }else{
         ivel->setVelocityMode();
-        commands=1000000;
+        commands=100.0;
         ivel->setRefAccelerations(commands.data());
     }
 
@@ -169,7 +172,7 @@ void iCubJointInterface::set_eyelid_angle(double angle){
         Bottle &cmd = emotion_port[0].prepare();
         cmd.clear();
         cmd.addString(buf);
-        emotion_port[0].write();
+        emotion_port[0].writeStrict();
     }else{
         printf("> ERROR: no icub emotion output\n");
         exit(EXIT_FAILURE);
@@ -227,7 +230,7 @@ void iCubJointInterface::set_eyebrow_angle(int id){
         Bottle &cmd = emotion_port[port_id].prepare();
         cmd.clear();
         cmd.addString(cmd_s);
-        emotion_port[port_id].write();
+        emotion_port[port_id].writeStrict();
     }else{
         printf("> ERROR: no icub emotion output\n");
         exit(EXIT_FAILURE);
@@ -252,7 +255,7 @@ void iCubJointInterface::publish_target_position(int e){
     if (id == ICUB_ID_NECK_PAN){
         //PAN seems to be swapped
         store_joint(ICUB_ID_NECK_PAN, -joint_target[e]);
-    }else if ((id == ICUB_ID_EYES_LEFT_LR) || ( id == ICUB_ID_EYES_RIGHT_LR)){
+    }else if ((id == ICUB_ID_EYES_PAN) || ( id == ICUB_ID_EYES_VERGENCE)){
         //icub handles eyes differently, we have to set pan angle + vergence
         float pan      = (joint_target[ID_EYES_LEFT_LR] + joint_target[ID_EYES_RIGHT_LR]) / 2;
         float vergence = (joint_target[ID_EYES_LEFT_LR]  - joint_target[ID_EYES_RIGHT_LR]);
@@ -283,35 +286,47 @@ void iCubJointInterface::set_target_in_positionmode(int id, double value){
         printf("> ERROR: set_target_positionmode(id=%d, %3.2f) not supported for this id\n",id,value);
         return;
     }
-#if 0
-    //set speed cacluated as in velocity + set position -> replicates smoothmotion from flobi?!
-
-    //first: calculate necessary speed to reach the given target within the next clock tick:
-    double distance = value - target_angle_previous[id];
-    //make the motion smooth: we want to reach 85% of the target in the next iteration:
-    //calculate speed for that:
-    double speed = 0.85 * distance * ((double)MAIN_LOOP_FREQUENCY);
 
-    //set up speed for controller:
-    ipos->setRefSpeed(id, speed);
-#endif
-    //execute motion
+    // execute motion as position control cmd
     ipos->positionMove(id, value);
+
 }
 
 //! execute a move in velocity mode
 //! \param id of joint
 //! \param angle
 void iCubJointInterface::set_target_in_velocitymode(int id, double value){
+    // set speed cacluated as in velocity + set position -> replicates smoothmotion from flobi?!
     //first: calculate necessary speed to reach the given target within the next clock tick:
     double distance = value - target_angle_previous[id];
+
     //make the motion smooth: we want to reach 85% of the target in the next iteration:
     distance = 0.85 * distance;
+
+    //distance = -5.0 / 50.0;
+
     //calculate speed
-    double speed = distance * ((double)MAIN_LOOP_FREQUENCY);
+    //double speed = distance * ((double)MAIN_LOOP_FREQUENCY);
+
+
+
+    int e = convert_motorid_to_enum(id);
+    double speed = joint_target_speed[e];
+
+    double max = 150.0;
+    if (speed > max) speed = max;
+    if (speed < -max) speed = -max;
+
+    //speed = -speed;
+
+
     //execute:
-    ivel->velocityMove(id, speed);
-    //if (id == ICUB_ID_NECK_PAN) printf("> VEL now=%3.2f target=%3.2f --> dist=%3.2f speed=%3.2f\n",target_angle_previous[id],value,distance,speed);
+    //ivel->velocityMove(id, speed);
+    if ((id == ICUB_ID_NECK_PAN)  || (id == ICUB_ID_EYES_BOTH_UD) || (id == ICUB_ID_NECK_TILT) || (id == ICUB_ID_EYES_BOTH_UD) ||  (id == ICUB_ID_NECK_TILT) ){
+        if (id == ICUB_ID_NECK_PAN) speed = -speed;
+        ivel->velocityMove(id, speed);
+        printf("> VEL now=%3.2f target=%3.2f --> dist=%3.2f speed=%3.2f\n",target_angle_previous[id],value,distance,speed);
+    }
 
     target_angle_previous[id] = get_ts_position(convert_motorid_to_enum(id)).get_newest_value();
 }
@@ -416,7 +431,7 @@ void iCubJointInterface::set_mouth(){
     Bottle &cmd = emotion_port[3].prepare();
     cmd.clear();
     cmd.addString(buf);
-    emotion_port[3].write();
+    emotion_port[3].writeStrict();
 
 
     //store joint values which we do not handle on icub here:
@@ -451,25 +466,25 @@ void iCubJointInterface::fetch_position(int id, double value, double timestamp){
             JointInterface::store_incoming_position(ID_EYES_RIGHT_LID_UPPER, lid_angle, timestamp);
             break;
 
-        case(2):
+        case(ICUB_ID_NECK_PAN):
             //PAN is inverted!
             JointInterface::store_incoming_position(ID_NECK_PAN, -value, timestamp);
             break;
 
-        case(0):
+        case(ICUB_ID_NECK_TILT):
             JointInterface::store_incoming_position(ID_NECK_TILT, value, timestamp);
             break;
 
-        case(1):
+        case(ICUB_ID_NECK_ROLL):
             JointInterface::store_incoming_position(ID_NECK_ROLL, value, timestamp);
             break;
 
-        case(3):
+        case(ICUB_ID_EYES_BOTH_UD):
             JointInterface::store_incoming_position(ID_EYES_BOTH_UD, value, timestamp);
             break;
 
         //icub handles eyes differently, we have to set pan angle + vergence
-        case(4): {//pan
+        case(ICUB_ID_EYES_PAN): {//pan
             last_pos_eye_pan = value;
             float left  = last_pos_eye_pan + last_pos_eye_vergence/2.0;
             float right = last_pos_eye_pan - last_pos_eye_vergence/2.0;
@@ -480,7 +495,7 @@ void iCubJointInterface::fetch_position(int id, double value, double timestamp){
             break;
         }
 
-        case(5): { //vergence
+        case(ICUB_ID_EYES_VERGENCE): { //vergence
             last_pos_eye_vergence = value;
             float left  = last_pos_eye_pan + last_pos_eye_vergence/2.0;
             float right = last_pos_eye_pan - last_pos_eye_vergence/2.0;
@@ -506,24 +521,25 @@ void iCubJointInterface::fetch_speed(int id, double value, double timestamp){
             printf("> ERROR: unhandled joint id %d\n",id);
             return;
 
-        case(2):
-            JointInterface::store_incoming_speed(ID_NECK_PAN, value, timestamp);
+        case(ICUB_ID_NECK_PAN):
+            //PAN IS INVERTED
+            JointInterface::store_incoming_speed(ID_NECK_PAN, -value, timestamp);
             break;
 
-        case(0):
+        case(ICUB_ID_NECK_TILT):
             JointInterface::store_incoming_speed(ID_NECK_TILT, value, timestamp);
             break;
 
-        case(1):
+        case(ICUB_ID_NECK_ROLL):
             JointInterface::store_incoming_speed(ID_NECK_ROLL, value, timestamp);
             break;
 
-        case(3):
+        case(ICUB_ID_EYES_BOTH_UD):
             JointInterface::store_incoming_speed(ID_EYES_BOTH_UD, value, timestamp);
             break;
 
         //icub handles eyes differently, we have to set pan angle + vergence
-        case(4): {//pan
+        case(ICUB_ID_EYES_PAN): {//pan
             last_vel_eye_pan = value;
             float left  = last_vel_eye_pan + last_vel_eye_vergence/2.0;
             float right = last_vel_eye_pan - last_vel_eye_vergence/2.0;
@@ -534,7 +550,7 @@ void iCubJointInterface::fetch_speed(int id, double value, double timestamp){
             break;
         }
 
-        case(5): { //vergence
+        case(ICUB_ID_EYES_VERGENCE): { //vergence
             last_vel_eye_pan = value;
             float left  = last_vel_eye_pan + last_vel_eye_vergence/2.0;
             float right = last_vel_eye_pan - last_vel_eye_vergence/2.0;
@@ -545,101 +561,64 @@ void iCubJointInterface::fetch_speed(int id, double value, double timestamp){
             break;
         }
     }
-/*
-    JointInterface::store_incoming_speed(ID_LIP_LEFT_UPPER, 0.0, timestamp);
-    JointInterface::store_incoming_speed(ID_LIP_LEFT_LOWER, 0.0, timestamp);
-    JointInterface::store_incoming_speed(ID_LIP_CENTER_UPPER, 0.0, timestamp);
-    JointInterface::store_incoming_speed(ID_LIP_CENTER_LOWER, 0.0, timestamp);
-    JointInterface::store_incoming_speed(ID_LIP_RIGHT_UPPER, 0.0, timestamp);
-    JointInterface::store_incoming_speed(ID_LIP_RIGHT_LOWER, 0.0, timestamp);
-
-    JointInterface::store_incoming_speed(ID_EYES_LEFT_LID_LOWER, 0.0, timestamp);
-    JointInterface::store_incoming_speed(ID_EYES_LEFT_LID_UPPER, 0.0, timestamp);
-    JointInterface::store_incoming_speed(ID_EYES_LEFT_BROW, 0.0, timestamp);
-
-    JointInterface::store_incoming_speed(ID_EYES_RIGHT_LID_LOWER, 0.0, timestamp);
-    JointInterface::store_incoming_speed(ID_EYES_RIGHT_LID_UPPER,0.0, timestamp);
-    JointInterface::store_incoming_speed(ID_EYES_RIGHT_BROW, 0.0, timestamp);*/
-    /*
-    //fetch enum id:
-    int e = convert_motorid_to_enum(device->get_device_id());
-    if (e == -1){
-        return; //we are not interested in that data, so we just return here
-    }
+}
 
-    JointInterface::store_incoming_speed(e, device->get_register(XSC3_REGISTER_MOTORSPEED), timestamp);*/
+void iCubJointInterface::set_joint_enable_state(int e, bool enable) {
+    int icub_jointid = -1;
 
-}
-/*
-//! conversion table for humotion motor ids to our ids:
-//! \param enum from JointInterface::JOINT_ID_ENUM
-//! \return int value of motor id
-int HumotionJointInterface::convert_enum_to_motorid(int e){
-    enum_id_bimap_t::right_const_iterator it = enum_id_bimap.right.find(e);
-    if(it == enum_id_bimap.right.end()) {
-        //key does not exists, we are not interested in that dataset, return -1
-        return -1;
-    }
+    switch(e){
+        default:
+            break;
 
-    return it->second;
-}
+    case(ID_NECK_PAN):
+        icub_jointid = ICUB_ID_NECK_PAN;
+        break;
 
+    case(ID_NECK_TILT):
+        icub_jointid = ICUB_ID_NECK_TILT;
+        break;
 
-//! conversion table for our ids to humotion motor ids:
-//! \param  int value of motor id
-//! \return enum from JointInterface::JOINT_ID_ENUM
-int HumotionJointInterface::convert_motorid_to_enum(int id){
-    enum_id_bimap_t::left_const_iterator it = enum_id_bimap.left.find(id);
-    if(it == enum_id_bimap.left.end()) {
-        //key does not exists, we are not interested in that dataset, return -1
-        return -1;
+    case(ID_NECK_ROLL):
+        icub_jointid = ICUB_ID_NECK_ROLL;
+        break;
+
+    case(ID_EYES_BOTH_UD):
+        icub_jointid = ICUB_ID_EYES_BOTH_UD;
+        break;
+
+    // icub handles eyes as pan angle + vergence...
+    // -> hack: left eye enables pan and right eye enables vergence
+    case(ID_EYES_LEFT_LR):
+        icub_jointid = ICUB_ID_EYES_PAN;
+        break;
+
+    case(ID_EYES_RIGHT_LR):
+        icub_jointid = ICUB_ID_EYES_VERGENCE;
+        break;
     }
 
-    return it->second;
+    if (icub_jointid != -1) {
+        if (enable) {
+            amp->enableAmp(icub_jointid);
+            pid->enablePid(icub_jointid);
+        } else {
+            pid->disablePid(icub_jointid);
+            amp->disableAmp(icub_jointid);
+        }
+    }
 }
-*/
 
 //! prepare and enable a joint
 //! NOTE: this should also prefill the min/max positions for this joint
 //! \param the enum id of a joint
 void iCubJointInterface::enable_joint(int e){
-    //FIXME ADD THIS:
-    // enable the amplifier and the pid controller on each joint
-    /*for (i = 0; i < nj; i++) {
-       amp->enableAmp(i);
-       pid->enablePid(i);
-    }*/
-
-
-    //set up smooth motion controller
-    //step1: set up framerate
-    //dev->set_register_blocking(XSC3_REGISTER_PID_RAMP, framerate, true);
-
-    //step2: set controllertype:
-    //printf("> activating smooth motion control for joint id 0x%02X (%s)\n", motor_id, joint_name.c_str());
-    //dev->set_register_blocking(XSC3_REGISTER_PID_CONTROLLER, XSC3_PROTOCOL_PID_CONTROLLERTYPE_SMOOTH_PLAYBACK, true);
-
-    //step3: set pid controller:
-    /*if ((e == ID_LIP_LEFT_UPPER) || (e == ID_LIP_LEFT_LOWER) || (e == ID_LIP_CENTER_UPPER) || (e == ID_LIP_CENTER_LOWER) || (e == ID_LIP_RIGHT_UPPER) || (e == ID_LIP_RIGHT_LOWER)){
-        printf("> fixing PID i controller value for smooth motion (FIXME: restore old value!!)\n");
-        dev->set_register_blocking(XSC3_REGISTER_CONST_I, 10, true);
-    }*/
-
-    //uint16_t result = dev->get_register_blocking_raw(XSC3_REGISTER_PID_CONTROLLER);
-
-    //check if setting pid controllertype was successfull:
-    /*if (result != XSC3_PROTOCOL_PID_CONTROLLERTYPE_SMOOTH_PLAYBACK){
-        printf("> failed to set smooth motion controller for joint %s (res=0x%04X)\n",joint_name.c_str(),result);
-        exit(1);
-    }*/
-
-    //fetch min/max:
-   // init_joint(e);
-
-    //ok fine, now enable motor:
-    //printf("> enabling motor %s\n", joint_name.c_str());
-    //dev->set_register_blocking(XSC3_REGISTER_STATUS, XSC3_PROTOCOL_STATUS_BITMASK_MOTOR_ENABLE, true);
+    set_joint_enable_state(e, true);
+}
 
+//! shutdown and disable a joint
+//! \param the enum id of a joint
+void iCubJointInterface::disable_joint(int e){
+    set_joint_enable_state(e, false);
 }
 
 void iCubJointInterface::store_min_max(IControlLimits *ilimits, int id, int e){
@@ -652,15 +631,15 @@ void iCubJointInterface::store_min_max(IControlLimits *ilimits, int id, int e){
 //! initialise a joint (set up controller mode etc)
 //! \param joint enum
 void iCubJointInterface::init_joints(){
-    store_min_max(ilimits, 0, ID_NECK_TILT);
-    store_min_max(ilimits, 1, ID_NECK_ROLL);
-    store_min_max(ilimits, 2, ID_NECK_PAN);
-    store_min_max(ilimits, 3, ID_EYES_BOTH_UD);
+    store_min_max(ilimits, ICUB_ID_NECK_TILT, ID_NECK_TILT);
+    store_min_max(ilimits, ICUB_ID_NECK_ROLL, ID_NECK_ROLL);
+    store_min_max(ilimits, ICUB_ID_NECK_PAN, ID_NECK_PAN);
+    store_min_max(ilimits, ICUB_ID_EYES_BOTH_UD, ID_EYES_BOTH_UD);
 
     //icub handles eyes differently, we have to set pan angle + vergence
     double pan_min, pan_max, vergence_min, vergence_max;
-    ilimits->getLimits(4, &pan_min, &pan_max);
-    ilimits->getLimits(5, &vergence_min, &vergence_max);
+    ilimits->getLimits(ICUB_ID_EYES_PAN, &pan_min, &pan_max);
+    ilimits->getLimits(ICUB_ID_EYES_VERGENCE, &vergence_min, &vergence_max);
 
     //this is not 100% correct, should be fixed:
     joint_min[ID_EYES_LEFT_LR] = pan_min; // - vergence_max/2;
@@ -695,27 +674,3 @@ void iCubJointInterface::init_joints(){
 
 
 }
-
-//! shutdown and disable a joint
-//! \param the enum id of a joint
-void iCubJointInterface::disable_joint(int e){
-    /*
-        //first: convert humotion enum to our enum:
-        int motor_id = convert_enum_to_motorid(e);
-        if (motor_id == -1){
-            return; //we are not interested in that data, so we just return here
-        }
-
-        //fetch device:
-        Device *dev = get_device(motor_id);
-        printf("> FIXME: ADD DISABLE CODE\n");
-        printf("> FIXME: ADD DISABLE CODE\n");
-        printf("> FIXME: ADD DISABLE CODE\n");
-        printf("> FIXME: ADD DISABLE CODE\n");
-        printf("> FIXME: ADD DISABLE CODE\n");
-        printf("> FIXME: ADD DISABLE CODE\n");
-        printf("> FIXME: ADD DISABLE CODE\n");
-        printf("> FIXME: ADD DISABLE CODE\n");
-        printf("> FIXME: ADD DISABLE CODE\n");
-        */
-}
diff --git a/examples/yarp_icub/src/main.cpp b/examples/yarp_icub/src/main.cpp
index f2d373f..bab7f2f 100644
--- a/examples/yarp_icub/src/main.cpp
+++ b/examples/yarp_icub/src/main.cpp
@@ -45,7 +45,7 @@ int main(int argc, char *argv[]){
 
     //create humotion interface
     iCubJointInterface *icub_jointinterface = new iCubJointInterface(scope);
-    humotion::server::Server *humotion_server = new humotion::server::Server("/icub", "RSB", icub_jointinterface);
+    humotion::server::Server *humotion_server = new humotion::server::Server("/icub", "ROS", icub_jointinterface);
     icub_jointinterface->run();
 
     while(humotion_server->ok()){
