humotion

#pragma once

#include <yarp/dev/ControlBoardInterfaces.h>

#include <yarp/os/Network.h>

#include "icub_jointinterface.h"

/*

 * #include <humotion/server/server.h>

#include <yarp/os/RateThread.h>

#include <yarp/os/Time.h>

#include <yarp/os/Property.h>

#include <yarp/os/Port.h>

#include <yarp/dev/ControlBoardInterfaces.h>

#include "icub_data_receiver.h"

*/

class iCubFaceInterface {

public:

    iCubFaceInterface(std::string scope);

    ~iCubFaceInterface();

    void set_eyelid_angle(double angle);

    void set_eyebrow_angle(int id, double *target_angle);

    void set_mouth(double *target_angle);

private:

    double lid_angle;

    int lid_opening_previous;

    int previous_mouth_state;

    double target_angle_previous[iCubJointInterface::ICUB_JOINT_ID_ENUM_SIZE];

    std::string scope;

    yarp::os::BufferedPort<yarp::os::Bottle> emotion_port[4];

};

#include "icub_faceinterface.h"

using namespace std;

using yarp::os::Network;

using yarp::os::Bottle;

//! constructor

iCubFaceInterface::iCubFaceInterface(string _scope) {

    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

iCubFaceInterface::~iCubFaceInterface(){

}

//! special command to set eyelid angle

//! \param angle in degrees

void iCubFaceInterface::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 iCubFaceInterface::set_eyebrow_angle(int id, double *target_angle){

    int port_id;

    if (id == iCubJointInterface::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==iCubJointInterface::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==iCubJointInterface::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 iCubFaceInterface::set_mouth(double *target_angle){

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

    int led_value = 0;

    //fetch center opening:

    double center_opening = target_angle[iCubJointInterface::ICUB_ID_LIP_CENTER_LOWER] -

            target_angle[iCubJointInterface::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[iCubJointInterface::ICUB_ID_LIP_CENTER_LOWER] +

            target_angle[iCubJointInterface::ICUB_ID_LIP_CENTER_UPPER])/2.0;

    double left_avg   = (target_angle[iCubJointInterface::ICUB_ID_LIP_LEFT_LOWER] +

            target_angle[iCubJointInterface::ICUB_ID_LIP_LEFT_UPPER])/2.0;

    double right_avg  = (target_angle[iCubJointInterface::ICUB_ID_LIP_RIGHT_LOWER] +

            target_angle[iCubJointInterface::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);

*/

}