Configuration procedure for tuning max/min values of servos to open/close eyelids

[pattacini]

Develop a configuration procedure within the iCubBlinker module that helps user tune up the max and min values of open-loop commands to be sent to servos to open/close the eyelids. Store these values inside the configuration file.

[alecive]

I will add a calib command to the rpc interface, and an .ini file to read these parameters for any robot. Question:

• from the pdf I understood that I need to communicate to the low-level via ASCII characters, that starts with S in order to address the eyelids' servo. For example, S5A will ask for opening (or, better, closing) the eyelids to their 90%.
• but then there is another command that starts with E that sets the servo angle corresponding to the maximum eyelid opening, in percentage of the full servo excursion. For example, E90 will set the maximum to 90% of the servo excursion.

So, my questions are:

• how they relate? S5A after E90 will close the eyes to the 90% of their 90%
• what should I address?
• are the values sent through E commands going to be saved for future run(s)?

[pattacini]

who knows :confused: let's investigate it through tests.

[myl1ne]

Haha ! What is this repo ? ^^ You have to find for each robot what are the max and min value... They vary on each of them. I never read about the E command, but I struggled quite a lot to find the right min max values. Basically you can RPC and send manually ascii starting with S and try to find the minimum and maximum... Then you note them down and you have what you whiched for...

Regarding this issue, I think there is a quite usefull class you could extract from the WYSIWYD mess: (The file I am pasting down there is located in wysiwyd\main\src\libraries\wrdac\include\wrdac\subsystems\subSystem_facialExpression.h ) the first part of the file can really facilitate your life if you try to play with the hexadecimal code. You can basically define an expression use boolean for the LEDS and double for the eyelid opening.

You can then just play this expression as a collection of ASCII commands.

/**
 * @defgroup facialExpression EFAA Helper Facial Expression
 *
 * @ingroup wrdac
 *
 * A simple structure and a client to manage the LEDs on the iCub face
 *
 * @author Stéphane Lallée
 */

/**
* \ingroup facialExpression
*
* Represent a LED configuration for the face.
*/
class FaceLED
{
private:
    static int getLEDDecimalValue(int i)
    {
        return 0x01<<i;
    }

    static std::string getHexCode(char startLetter, int decimalSum)
    {
        std::ostringstream codeValue;
        codeValue<< startLetter;
        if (decimalSum<16)
            codeValue<<'0';
        codeValue<<std::hex << decimalSum;
        return codeValue.str();
    }

    std::vector<bool> mouth;
    std::vector<bool> lEye;
    std::vector<bool> rEye;
    double            eyeLids;
    int               eyeLidsMin, eyeLidsMax;
public:

    FaceLED()
    {
        mouth.resize(6,false);
        lEye.resize(4,false);
        rEye.resize(4,false);
        eyeLids = 1.0;
        eyeLidsMin = 40;
        eyeLidsMax = 60;
    }

    /**
    * Define the robot specific range for the eyelids
    */
    void DefineEyelidsRange(int min, int max)
    {
        eyeLidsMin = min;
        eyeLidsMax = max;
    }

    /**
    * Define all the mouth LED status
    */
    void SetMouthLED(bool L1, bool L2, bool L3, bool L4, bool L5, bool L6)
    {
        mouth[0] = L1;
        mouth[1] = L2;
        mouth[2] = L3;
        mouth[3] = L4;
        mouth[4] = L5;
        mouth[5] = L6;
    }

    /**
    * Define the status of a single LED of the mouth
    */
    void SetMouthLED(int i, bool status)
    {
        mouth[i] = status;
    }

    /**
    * Define all the left eyebrow LED status
    */
    void SetLEyeLED(bool L1, bool L2, bool L3, bool L4)
    {
        lEye[0] = L1;
        lEye[1] = L2;
        lEye[2] = L3;
        lEye[3] = L4;
    }

    /**
    * Define the status of a single LED of the left eyebrow
    */
    void SetLEyeLED(int i, bool status)
    {
        lEye[i] = status;
    }

    /**
    * Define all the right eyebrow LED status
    */
    void SetREyeLED(bool L1, bool L2, bool L3, bool L4)
    {
        rEye[0] = L1;
        rEye[1] = L2;
        rEye[2] = L3;
        rEye[3] = L4;
    }

    /**
    * Define the status of a single LED of the left eyebrow
    */
    void SetREyeLED(int i, bool status)
    {
        rEye[i] = status;
    }

    /**
    * Define the status of a single LED of the left eyebrow
    */
    void SetEyeOpening(double i)
    {
        eyeLids = (std::max)(0.0,(std::min)(1.0,i));
    }

    /**
    * return the raw hexadecimal code for the mouth
    */
    std::string mouthCode()
    {
        int hexSum = 0;
        for(int i=0;i<6;i++)
        {
            if (mouth[i])
                hexSum += getLEDDecimalValue(i);
        }
        std::string code = getHexCode('M',hexSum);
        std::transform(code.begin(), code.end(),code.begin(), ::toupper);
        return code;
    }

    /**
    * return the raw hexadecimal code for the right eye
    */
    std::string rightEyeCode()
    {
        int hexSum = 0;
        for(int i=0;i<4;i++)
        {
            if (rEye[i])
                hexSum += getLEDDecimalValue(i);
        }
        std::string code = getHexCode('R',hexSum);
        std::transform(code.begin(), code.end(),code.begin(), ::toupper);
        return code;
    }

    /**
    * return the raw hexadecimal code for the left eye
    */
    std::string leftEyeCode()
    {
        int hexSum = 0;
        for(int i=0;i<4;i++)
        {
            if (lEye[i])
                hexSum += getLEDDecimalValue(i);
        }

        std::string code = getHexCode('L',hexSum);
        std::transform(code.begin(), code.end(),code.begin(), ::toupper);
        return code;
    }
    /**
    * return the raw hexadecimal code for the eyes opening
    */
    std::string eyesOpeningCode()
    {   int maxValue = eyeLidsMax;
        int minValue = eyeLidsMin;
        int scaledValue = (int)(minValue + (maxValue - minValue) * eyeLids);
        std::stringstream strstr;
        strstr<<'S'<<scaledValue;
        std::string code = strstr.str(); //getHexCode('S',scaledValue);
        //std::transform(code.begin(), code.end(),code.begin(), ::toupper);
        return code;
    }
};

/**
* \ingroup facialExpression
*
* Client to send FaceLED configurations to the robot.
*/
class FaceLEDClient
{
private:
    yarp::os::Port toEmotionInterface;

public:
    FaceLEDClient(yarp::os::ResourceFinder &rf)
    {
        std::string moduleName = rf.check("name",yarp::os::Value("faceLED")).asString().c_str();
        std::string portName = "/";
        portName +=moduleName.c_str();
        portName +="/emotions:o";
        toEmotionInterface.open(portName.c_str());
        connect();
    }

    FaceLEDClient(const std::string &moduleName)
    {
        std::string portName = "/";
        portName +=moduleName.c_str();
        portName +="/emotions:o";
        toEmotionInterface.open(portName.c_str());
    }

    /**
    * Interrupt port communication
    */
    void interrupt()
    {
        toEmotionInterface.interrupt();
    }

    /**
    * Close port
    */
    void close()
    {
        toEmotionInterface.close();
    }
    /**
    * Try to connect to the emotion interface port ("/icub/face/emotions/in")
    */
    bool connect(const std::string &targetPort = "/icub/face/emotions/in")
    {
        return yarp::os::Network::connect(toEmotionInterface.getName(),targetPort.c_str());
    }

    /**
    * Make the iCub blink
    */
    void blink(double blink_time = 0.0750)
    {
        FaceLED led;
        led.SetEyeOpening(0);
        send(led, false, false, false , true);
        yarp::os::Time::delay(blink_time);
        led.SetEyeOpening(1);
        send(led, false, false, false , true);
    }

    /**
    * Send a facial expression to be displayed
    * @param face The FaceLED configuration to be displayed
    * @param sendMouth Should the mouth be updated (default=true)
    * @param sendEyeBrowR Should the right eybrow be updated (default=true)
    * @param sendEyeBrowL Should the left eybrow be updated (default=true)
    * @param sendEyeOpening Should the eyelids be updated (default=true)
    */
    void send(FaceLED face, bool sendMouth = true, bool sendEyeBrowR = true, bool sendEyeBrowL = true, bool sendEyeOpening = true)
    {
        yarp::os::Bottle cmd;

        //Set the mouth
        if (sendMouth)
        {
            cmd.addString("set");
            cmd.addString("raw");
            cmd.addString(face.mouthCode().c_str());
            toEmotionInterface.write(cmd);
            //cout<<"Sending..."<<cmd.toString().c_str()<<endl;
        }

        //lEye
        if (sendEyeBrowL)
        {
            cmd.clear();
            cmd.addString("set");
            cmd.addString("raw");
            cmd.addString(face.leftEyeCode().c_str());
            toEmotionInterface.write(cmd);
            //cout<<"Sending..."<<cmd.toString().c_str()<<endl;
        }

        //rEye
        if (sendEyeBrowR)
        {
            cmd.clear();
            cmd.addString("set");
            cmd.addString("raw");
            cmd.addString(face.rightEyeCode().c_str());
            toEmotionInterface.write(cmd);
            //cout<<"Sending..."<<cmd.toString().c_str()<<endl;
        }

        //opening
        if (sendEyeOpening)
        {
            cmd.clear();
            cmd.addString("set");
            cmd.addString("raw");
            cmd.addString(face.eyesOpeningCode().c_str());
            toEmotionInterface.write(cmd);
            //cout<<"Sending..."<<cmd.toString().c_str()<<endl;
        }
    }
};

/**
* \ingroup facialExpression
*
* Structure to store multiple facial expressions corresponding to various levels
* of an emotion.
*/
struct FacialEmotion
{
    std::map<double,FaceLED> intensity;

    /**
    * Return the closest template for a given intensity
    * @param value The current emotional intensity.
    */
    FaceLED getClosestFace(double value)
    {
        std::map<double,FaceLED>::iterator bestChoice = intensity.begin();
        for(std::map<double,FaceLED>::iterator it = intensity.begin(); it!=intensity.end(); it++)
        {
            if ( fabs(value - it->first) < fabs(value - bestChoice->first) )
                bestChoice = it;
        }
        return bestChoice->second;
    }
};

[alecive]

Hey @stephane-lallee , how are you doing?

Regarding the issue, basically E and U set the percentage of the servo angle for which the eyes will be fully open and fully closed respectively. They are basically the upper-lower range within which the S command can go. For example, I just set them as E99 and U70 on the blue robot. Then, the ASCII command you send via the S command will play within those limits.

My idea was to let the user fiddle with them, in order to then use the S commands inside my code in order to make something that works the same on any robot as soon as E and U are properly set. What do you think @stephane-lallee @pattacini ?

Problems are:

• I can ask for the status of the S variable, but I do not have any info about E and U
• the interface is not a proper rpc port as far as I understood, so I cannot interface with it in a sane way

[alecive]

Regarding your code, I will take a look at that, thanks! But what is it doing exactly? Because for the time being I don't need to handle the LEDs, but only the servo for the eyelids.

[pattacini]

@alecive this is the right road to purse, yep.

Even if you don't get any info on E and U, we aim to have a semi-automatic way to help user tune them up on the fly, at start-up, and then store those values somewhere for later usage - if this cannot be already achieved through the current interface.

E and U have been seen varying also on the same robot, over time, but very slowly, that is after days.

Please, while coding stick always to the K.I.S.S. principle :smirk:

[myl1ne]

@alecive I am fine thanks :) Just trying to come back guide my current project here toward a robotic flavour ;-)

If you want only the eyelids then you probably do not really need this class, as it is more a handy way to store a specific face configuration in something more intelligible than hexadecimal code ^^. And for the eyelid this code is actually straightforward...

But I think we could try to move the whole class to somewhere within the iCub, because honestly the way the face expression are handled is creepily old and low level...

Regarding to the E, U and S. One thing you may realize during the calibration and later play around is that the mechanical opening of the eye is not the same each time you send the same value: it depends of the previous state of the eyelid (at least it was the case in BCN).

It is not that obvious, but you will notice it if you try to interpolate at regular intervals between the min and max. In the end we found that only 3 positions (wide open, closed and in the middle) were reliable.

For example you can try something like (with min 0 and max 10) S00 -> S02 S10 -> S02

And it is likely that you will not get the same exact opening in both cases...

[pattacini]

@stephane-lallee the lack of movements repeatability is somehow expected since we don't have reliable feedback for that.

[pattacini]

Can we close this?

[alecive]

There is no automatic procedure up to now. It is only partially solved.

[serena-ivaldi]

Dear all, any news about this? I am trying to configure it for my robot iCubNancy01 but it seems not to work at all.. the eyelids are not moving at all (not even increasing the values sMIn sMax). How can I check if the eyelids are working correctly? This may be a cross issue between iCubBlinker and icub-support ...

[pattacini]

Hi @serena-ivaldi I think it's a hardware problem, so you'd better ask for help at icub-support. Btw, no progress on this issue, and I'm afraid we won't have resources to allocate for long 😞

[pattacini]

Likely dead end, closing.