humotion

#include "mekajointinterface.h"

using namespace std;

//WARNING: DO NOT CHANGE THIS; VELOCITYMODE IS NOT YET IMPLEMENTED

#define POSITION_CONTROL 1

void MekaJointInterface::incoming_jointstates(const sensor_msgs::JointState & msg){

    //joint_pos_global = msg.position[3];

    cout << msg;

}

//! constructor

MekaJointInterface::MekaJointInterface(string _scope) : humotion::server::JointInterface(){

    scope = _scope;

    //subscribe to meka joint states

    int argc = 0;

    ros::init(argc, (char**)NULL, "meka_humotion");

    ros::NodeHandle n;

    joint_state_subscriber = n.subscribe(scope + "joint_states", 150, &MekaJointInterface::incoming_jointstates , this);

    //add mapping from ids to enums:

    //this might look strange at the first sight but we need to have a generic

    //way to acces joints from libhumotion. therefore the lib uses its enum with ID_* enum ids

    //to access the joints. now we need to define a mapping to map those to our motor ids.

    //this is what we use the enum bimap for (convertion fro/to motorid is handled

    //by \sa convert_enum_to_motorid() and \sa convert_motorid_to_enum() lateron

/*

    //MOUTH

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_LIP_LEFT_UPPER,   ID_LIP_LEFT_UPPER));

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_LIP_LEFT_LOWER,   ID_LIP_LEFT_LOWER));

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_LIP_CENTER_UPPER, ID_LIP_CENTER_UPPER));

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_LIP_CENTER_LOWER, ID_LIP_CENTER_LOWER));

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_LIP_RIGHT_UPPER,  ID_LIP_RIGHT_UPPER));

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_LIP_RIGHT_LOWER,  ID_LIP_RIGHT_LOWER));

    //NECK

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_NECK_PAN,    ID_NECK_PAN));

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_NECK_TILT,   ID_NECK_TILT));

    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_BOTH_UD,   ID_EYES_BOTH_UD));

    //EYELIDS

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_LEFT_LID_LOWER, ID_EYES_LEFT_LID_LOWER));

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_LEFT_LID_UPPER, ID_EYES_LEFT_LID_UPPER));

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_LEFT_BROW, ID_EYES_LEFT_BROW));

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_RIGHT_LID_LOWER, ID_EYES_RIGHT_LID_LOWER));

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_RIGHT_LID_UPPER,ID_EYES_RIGHT_LID_UPPER));

    enum_id_bimap.insert(enum_id_bimap_entry_t(ICUB_ID_EYES_RIGHT_BROW, ID_EYES_RIGHT_BROW));

    Property options;

    options.put("device", "remote_controlboard");

    options.put("local", "/local/head");

    options.put("remote", scope+"/head");

    dd.open(options);

    //fetch views:

    dd.view(iencs);

    dd.view(ipos);

    dd.view(ivel);

    dd.view(ilimits);

    if ( (!iencs) || (!ipos) || (!ilimits) || (!ivel)){

        printf("> ERROR: failed to open icub views\n");

        exit(EXIT_FAILURE);

    }

    int joints;

    //tell humotion about min/max joint values:

    init_joints();

    iencs->getAxes(&joints);

    positions.resize(joints);

    velocities.resize(joints);

    commands.resize(joints);

    //set position mode:

    if (POSITION_CONTROL){

        commands=200000.0;

        ipos->setRefAccelerations(commands.data());

        ipos->setPositionMode();

    }else{

        ivel->setVelocityMode();

        commands=1000000;

        ivel->setRefAccelerations(commands.data());

    }

    //attach to facial expressions:

    string emotion_scope = scope + "/face/raw/in";

    printf("> opening connection to %s\n", emotion_scope.c_str());

    for(int i=0; i<4; i++){

        //strange, if we use one output port only the first command is executed?! flushing issues?

        string emotion_port_out = "/emotionwriter" + to_string(i);

        if (!emotion_port[i].open(emotion_port_out.c_str())){

            printf("> ERROR: failed to open to %s\n",emotion_port_out.c_str());

            exit(EXIT_FAILURE);

        }

        if (!Network::connect(emotion_port_out.c_str(), emotion_scope.c_str())){

            printf("> ERROR: failed to connect emotion ports\n");

            exit(EXIT_FAILURE);

        }

    }

    */

}

//! destructor

MekaJointInterface::~MekaJointInterface(){

}

void MekaJointInterface::run(){

   //iCubDataReceiver *data_receiver = new iCubDataReceiver(10/*1000.0 / MAIN_LOOP_FREQUENCY*/, iencs, this);

   //data_receiver->start();

}

//! set the target position of a joint

//! \param enum id of joint

//! \param float value

void MekaJointInterface::publish_target_position(int e){

    #if 0

    //first: convert humotion enum to our enum:

    int id = convert_enum_to_motorid(e);

    if (id == -1){

        return; //we are not interested in that data, so we just return here

    }

    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)){

        //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]);

        //printf("EYEDBG %3.2f %3.2f --_> pan %3.2f verg=%3.2f\n",joint_target[ID_EYES_LEFT_LR], joint_target[ID_EYES_RIGHT_LR],pan,vergence);

        store_joint(ICUB_ID_EYES_PAN, pan);

        store_joint(ICUB_ID_EYES_VERGENCE, vergence);

    }else{

        store_joint(id, joint_target[e]);

    }

    #endif

}

//! actually execute the scheduled motion commands

void MekaJointInterface::execute_motion(){

    #if 0

    // set up neck and eye motion commands:

    if (POSITION_CONTROL){

        //position control

        for(int i=ICUB_ID_NECK_TILT; i<=ICUB_ID_EYES_VERGENCE; i++){

            set_target_in_positionmode(i, target_angle[i]);

        }

    }else{

        //velocity control

        for(int i=ICUB_ID_NECK_TILT; i<=ICUB_ID_EYES_VERGENCE; i++){

            set_target_in_velocitymode(i, target_angle[i]);

        }

    }

    //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:

    set_eyelid_angle(target_angle[ICUB_ID_EYES_RIGHT_LID_UPPER]);

    //eyebrows are set using a special command as well:

    set_eyebrow_angle(ICUB_ID_EYES_LEFT_BROW);

    set_eyebrow_angle(ICUB_ID_EYES_RIGHT_BROW);

    //mouth

    set_mouth();

    #endif

}

//! 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 MekaJointInterface::enable_joint(int e){

#if 0

    //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);

#endif

}

//! shutdown and disable a joint

//! \param the enum id of a joint

void MekaJointInterface::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");

        */

}

#if 0

//! conversion table for humotion motor ids to our ids:

//! \param enum from JointInterface::JOINT_ID_ENUM

//! \return int value of motor id

int iCubJointInterface::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;

    }

    return it->second;

}

//! conversion table for our ids to humotion motor ids:

//! \param  int value of motor id

//! \return enum from JointInterface::JOINT_ID_ENUM

int iCubJointInterface::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;

    }

    return it->second;

}

//! special command to set eyelid angle

//! \param angle in degrees

void iCubJointInterface::set_eyelid_angle(double angle){

    if (emotion_port[0].getOutputCount()>0){

        //try to set the value based on the upper one

        //some guesses from the sim: S30 = 0° / S40 = 10°

        int opening = (25.0 + 0.8*angle);

        opening = min(48, max(24, opening));

        if (opening == lid_opening_previous){

            //no update necessary

            return;

        }

        lid_angle = angle;

        lid_opening_previous = opening;

        char buf[20];

        sprintf(buf, "S%2d", opening);

        //printf("> SETTING EYELID '%s' (%f -> %d)\n",buf,angle,opening);

        Bottle &cmd = emotion_port[0].prepare();

        cmd.clear();

        cmd.addString(buf);

        emotion_port[0].write();

    }else{

        printf("> ERROR: no icub emotion output\n");

        exit(EXIT_FAILURE);

    }

}

//! special command to set the eyebrow angle

//! \param id {0=left, 1=right)

//! \param angle in degrees

void iCubJointInterface::set_eyebrow_angle(int id){

    int port_id;

    if (id == ICUB_ID_EYES_LEFT_BROW){

        port_id = 1;

    }else{

        port_id = 2;

    }

    if (emotion_port[port_id].getOutputCount()>0){

        double angle = target_angle[id];

        int icub_val = 0;

        //swap rotation direction:

        if (id==ICUB_ID_EYES_LEFT_BROW) angle = -angle;

        //convert to icub representation

        if (angle < -20){

            icub_val = 1;

        }else if (angle<10){

            icub_val = 2;

        }else if (angle<20){

            icub_val = 4;

        }else{

            icub_val = 8;

        }

        //make sure to update only on new values:

        if (icub_val == target_angle_previous[id]){

                //no updata necessary

                return;

        }

        //store actual value:

        target_angle_previous[id] = icub_val;

        string cmd_s;

        if (id==ICUB_ID_EYES_LEFT_BROW){

            cmd_s = "L0" + to_string(icub_val);

        }else{

            cmd_s = "R0" + to_string(icub_val);

        }

        printf("> SETTING EYEBROW %d (%f -> %s)\n",id,angle,cmd_s.c_str());

        Bottle &cmd = emotion_port[port_id].prepare();

        cmd.clear();

        cmd.addString(cmd_s);

        emotion_port[port_id].write();

    }else{

        printf("> ERROR: no icub emotion output\n");

        exit(EXIT_FAILURE);

    }

}

void iCubJointInterface::run(){

    iCubDataReceiver *data_receiver = new iCubDataReceiver(10/*1000.0 / MAIN_LOOP_FREQUENCY*/, iencs, this);

    data_receiver->start();

}

//! set the target position of a joint

//! \param enum id of joint

//! \param float value

void iCubJointInterface::publish_target_position(int e){

    //first: convert humotion enum to our enum:

    int id = convert_enum_to_motorid(e);

    if (id == -1){

        return; //we are not interested in that data, so we just return here

    }

    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)){

        //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]);

        //printf("EYEDBG %3.2f %3.2f --_> pan %3.2f verg=%3.2f\n",joint_target[ID_EYES_LEFT_LR], joint_target[ID_EYES_RIGHT_LR],pan,vergence);

        store_joint(ICUB_ID_EYES_PAN, pan);

        store_joint(ICUB_ID_EYES_VERGENCE, vergence);

    }else{

        store_joint(id, joint_target[e]);

    }

}

//! set the target position of a joint

//! \param id of joint

//! \param float value of position

void iCubJointInterface::store_joint(int id, float value){

    //printf("> set joint %d = %f\n",id,value);

    target_angle[id] = value;

    //ipos->positionMove(id, value);

}

//! execute a move in position mode

//! \param id of joint

//! \param angle

void iCubJointInterface::set_target_in_positionmode(int id, double value){

    if (id>ICUB_ID_EYES_VERGENCE){

        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

    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){

    //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;

    //calculate speed

    double speed = distance * ((double)MAIN_LOOP_FREQUENCY);

    //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);

    target_angle_previous[id] = get_ts_position(convert_motorid_to_enum(id)).get_newest_value();

}

//! actually execute the scheduled motion commands

void iCubJointInterface::execute_motion(){

    // set up neck and eye motion commands:

    if (POSITION_CONTROL){

        //position control

        for(int i=ICUB_ID_NECK_TILT; i<=ICUB_ID_EYES_VERGENCE; i++){

            set_target_in_positionmode(i, target_angle[i]);

        }

    }else{

        //velocity control

        for(int i=ICUB_ID_NECK_TILT; i<=ICUB_ID_EYES_VERGENCE; i++){

            set_target_in_velocitymode(i, target_angle[i]);

        }

    }

    //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:

    set_eyelid_angle(target_angle[ICUB_ID_EYES_RIGHT_LID_UPPER]);

    //eyebrows are set using a special command as well:

    set_eyebrow_angle(ICUB_ID_EYES_LEFT_BROW);

    set_eyebrow_angle(ICUB_ID_EYES_RIGHT_BROW);

    //mouth

    set_mouth();

}

void iCubJointInterface::set_mouth(){

    //convert from 6DOF mouth displacement to icub leds:

    int led_value = 0;

    //fetch center opening:

    double center_opening = target_angle[ICUB_ID_LIP_CENTER_LOWER] - target_angle[ICUB_ID_LIP_CENTER_UPPER];

    bool mouth_open = (center_opening>15.0)?true:false;

    //side of mouth high or low?

    double center_avg = (target_angle[ICUB_ID_LIP_CENTER_LOWER] + target_angle[ICUB_ID_LIP_CENTER_UPPER])/2.0;

    double left_avg   = (target_angle[ICUB_ID_LIP_LEFT_LOWER] + target_angle[ICUB_ID_LIP_LEFT_UPPER])/2.0;

    double right_avg  = (target_angle[ICUB_ID_LIP_RIGHT_LOWER] + target_angle[ICUB_ID_LIP_RIGHT_UPPER])/2.0;

    //happy, neutral or sad?

    double diff_l = center_avg - left_avg;

    double diff_r = center_avg - right_avg;

    double diff   = (diff_l+diff_r)/2.0;

    if (diff > 2.0){

        if (mouth_open){

            led_value = 0x14;

        }else{

            if (diff > 2.6){

                led_value = 0x0A;

            }else{

                led_value = 0x0B;

            }

        }

    }else if (diff < -3.0){

        if (mouth_open){

            led_value = 0x06;

        }else{

            led_value = 0x18;

        }

    }else if (diff < -2.0){

        if (mouth_open){

            led_value = 0x04; //0x25;

        }else{

            led_value = 0x08;

        }

    }else{

        if (mouth_open){

            led_value = 0x16;

        }else{

            led_value = 0x08;

        }

    }

    if (led_value == previous_mouth_state){

        //no update necessary

        return;

    }

    previous_mouth_state = led_value;

    //convert to string:

    char buf[10];

    sprintf(buf, "M%02X",led_value);

    /*printf("> sending mouth '%s'\n",buf);

    printf("> mouth angles: %3.2f %3.2f %3.2f\n",target_angle[ICUB_ID_LIP_LEFT_UPPER],target_angle[ICUB_ID_LIP_CENTER_UPPER],target_angle[ICUB_ID_LIP_RIGHT_UPPER]);

    printf("  mouth         %3.2f %3.2f %3.2f\n",target_angle[ICUB_ID_LIP_LEFT_LOWER],target_angle[ICUB_ID_LIP_CENTER_LOWER],target_angle[ICUB_ID_LIP_RIGHT_LOWER]);

    printf("  mouth  open=%3.2f diff=%3.2f\n", center_opening, diff);*/

    //add mouth:

    Bottle &cmd = emotion_port[3].prepare();

    cmd.clear();

    cmd.addString(buf);

    emotion_port[3].write();

    //store joint values which we do not handle on icub here:

    double timestamp = get_timestamp_ms();

    JointInterface::store_incoming_position(ID_LIP_LEFT_UPPER,   target_angle[ICUB_ID_LIP_LEFT_UPPER], timestamp);

    JointInterface::store_incoming_position(ID_LIP_LEFT_LOWER,   target_angle[ICUB_ID_LIP_LEFT_LOWER], timestamp);

    JointInterface::store_incoming_position(ID_LIP_CENTER_UPPER, target_angle[ICUB_ID_LIP_CENTER_UPPER], timestamp);

    JointInterface::store_incoming_position(ID_LIP_CENTER_LOWER, target_angle[ICUB_ID_LIP_CENTER_LOWER], timestamp);

    JointInterface::store_incoming_position(ID_LIP_RIGHT_UPPER,  target_angle[ICUB_ID_LIP_RIGHT_UPPER], timestamp);

    JointInterface::store_incoming_position(ID_LIP_RIGHT_LOWER,  target_angle[ICUB_ID_LIP_RIGHT_LOWER], timestamp);

}

double iCubJointInterface::get_timestamp_ms(){

    struct timespec spec;

    clock_gettime(CLOCK_REALTIME, &spec);

    return spec.tv_sec*1000 + spec.tv_nsec / 1.0e6;

}

//! set the current position of a joint

//! \param id of joint

//! \param float value of position

//! \param double timestamp

void iCubJointInterface::fetch_position(int id, double value, double timestamp){

    //store joint based on id:

    switch(id){

        default:

            printf("> ERROR: unhandled joint id %d\n",id);

            return;

        case(100):

            JointInterface::store_incoming_position(ID_EYES_RIGHT_LID_UPPER, lid_angle, timestamp);

            break;

        case(2):

            //PAN is inverted!

            JointInterface::store_incoming_position(ID_NECK_PAN, -value, timestamp);

            break;

        case(0):

            JointInterface::store_incoming_position(ID_NECK_TILT, value, timestamp);

            break;

        case(1):

            JointInterface::store_incoming_position(ID_NECK_ROLL, value, timestamp);

            break;

        case(3):

            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

            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;

            //printf("> eye: pan=%3.2f vergence=%3.2f --> L=%3.2f R=%3.2f\n", last_pos_eye_pan, last_pos_eye_vergence, left, right);

            JointInterface::store_incoming_position(ID_EYES_LEFT_LR, left, timestamp);

            JointInterface::store_incoming_position(ID_EYES_RIGHT_LR, right, timestamp);

            break;

        }

        case(5): { //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;

            //printf("> eye: pan=%3.2f vergence=%3.2f --> L=%3.2f R=%3.2f\n", last_pos_eye_pan, last_pos_eye_vergence, left, right);

            JointInterface::store_incoming_position(ID_EYES_LEFT_LR, left, timestamp);

            JointInterface::store_incoming_position(ID_EYES_RIGHT_LR, right, timestamp);

            break;

        }

    }

}

//! set the current speed of a joint

//! \param enum id of joint

//! \param float value of speed

//! \param double timestamp

void iCubJointInterface::fetch_speed(int id, double value, double timestamp){

    switch(id){

        default:

            printf("> ERROR: unhandled joint id %d\n",id);

            return;

        case(2):

            JointInterface::store_incoming_speed(ID_NECK_PAN, value, timestamp);

            break;

        case(0):

            JointInterface::store_incoming_speed(ID_NECK_TILT, value, timestamp);

            break;

        case(1):

            JointInterface::store_incoming_speed(ID_NECK_ROLL, value, timestamp);

            break;

        case(3):

            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

            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;

            //printf("> eye: velocity pan=%3.2f vergence=%3.2f --> L=%3.2f R=%3.2f\n", last_vel_eye_pan, last_vel_eye_vergence, left, right);

            JointInterface::store_incoming_speed(ID_EYES_LEFT_LR, left, timestamp);

            JointInterface::store_incoming_speed(ID_EYES_RIGHT_LR, right, timestamp);

            break;

        }

        case(5): { //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;

            //printf("> eye: velocity pan=%3.2f vergence=%3.2f --> L=%3.2f R=%3.2f\n", last_vel_eye_pan, last_vel_eye_vergence, left, right);

            JointInterface::store_incoming_speed(ID_EYES_LEFT_LR, left, timestamp);

            JointInterface::store_incoming_speed(ID_EYES_RIGHT_LR, right, 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);*/

}

/*

//! 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;

    }

    return it->second;

}

//! 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;

    }

    return it->second;

}

*/

//! 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);

}

void iCubJointInterface::store_min_max(IControlLimits *ilimits, int id, int e){

    double min, max;

    ilimits->getLimits(id, &min, &max);

    joint_min[e] = min;

    joint_max[e] = max;

}

//! 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);

    //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);

    //this is not 100% correct, should be fixed:

    joint_min[ID_EYES_LEFT_LR] = pan_min; // - vergence_max/2;

    joint_max[ID_EYES_LEFT_LR] = pan_max; // - vergence_max/2;

    joint_min[ID_EYES_RIGHT_LR] = joint_min[ID_EYES_LEFT_LR];

    joint_max[ID_EYES_RIGHT_LR] = joint_max[ID_EYES_LEFT_LR];

    //eyelids:

    joint_min[ID_EYES_RIGHT_LID_UPPER] = -50; //24-30;

    joint_max[ID_EYES_RIGHT_LID_UPPER] = 50; //48-30;

    lid_angle = joint_max[ID_EYES_RIGHT_LID_UPPER];

    //eyebrows:

    joint_min[ID_EYES_LEFT_BROW] = -50;

    joint_max[ID_EYES_LEFT_BROW] = 50;

    joint_min[ID_EYES_RIGHT_BROW] = joint_min[ID_EYES_LEFT_BROW];

    joint_max[ID_EYES_RIGHT_BROW] = joint_max[ID_EYES_LEFT_BROW];

    //mouth:

    joint_min[ID_LIP_CENTER_UPPER] = 5;

    joint_max[ID_LIP_CENTER_UPPER] = 50;

    joint_min[ID_LIP_CENTER_LOWER] = 5;

    joint_max[ID_LIP_CENTER_LOWER] = 50;

    joint_min[ID_LIP_LEFT_UPPER] = 5;

    joint_max[ID_LIP_LEFT_UPPER] = 50;

    joint_min[ID_LIP_LEFT_LOWER] = 5;

    joint_max[ID_LIP_LEFT_LOWER] = 50;

    joint_min[ID_LIP_RIGHT_UPPER] = 5;

    joint_max[ID_LIP_RIGHT_UPPER] = 50;

    joint_min[ID_LIP_RIGHT_LOWER] = 5;

    joint_max[ID_LIP_RIGHT_LOWER] = 50;

}

//! 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");

        */

}

#endif