humotion

#include "icub_jointinterface.h"

#include <yarp/os/Property.h>

using namespace yarp::dev;

using namespace yarp::sig;

using namespace yarp::os;

using namespace std;

/*running:

/media/local_data/sschulz/iros15/icub-nightly/share/iCub/contexts/simConfig:> iCub_SIM

/media/local_data/sschulz/iros15/icub-nightly/share/iCub/contexts/simFaceExpressions:> ../../../../bin/simFaceExpressions

yarp connect /face/eyelids /icubSim/face/eyelids

yarp connect /face/image/out /icubSim/texture/face

TEST: yarp write /writer /icubSim/face/raw/in

http://wiki.icub.org/wiki/Motor_control

*/

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

#define POSITION_CONTROL 0

//! constructor

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

    scope = _scope;

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

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

    dd.view(pid);

    dd.view(amp);

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

        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=100.0;

        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

iCubJointInterface::~iCubJointInterface(){

}

//! 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].writeStrict();

    }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].writeStrict();

    }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_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]);

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

    }

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

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

}

//! 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].writeStrict();

    //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(ICUB_ID_NECK_PAN):

            //PAN is inverted!

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

            break;

        case(ICUB_ID_NECK_TILT):

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

            break;

        case(ICUB_ID_NECK_ROLL):

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

            break;

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

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