How to use the iCubFinger forward kinematics

[pattacini]

The iCub::iKin::iCubFinger object is designed to compute the forward kinematics of the iCub fingers.

Since there is no direct mapping among the hand encoders streamed by the standard state port, e.g. /icub/left_arm/state:o, and the joints of the chain representing the specific finger, we can make use of iCub::iKin::iCubFinger::getChainJoints() helper method as done here below:

yarp::os::Property options;
options.put("device","remote_controlboard");
options.put("remote","/icub/left_arm");
options.put("local","/local/left_arm");

yarp::dev::PolyDriver driver;
iCub::iKin::iCubFinger finger("left_index");

if (driver.open(options))
{
    yarp::dev::IEncoders *iencs;
    if (driver.view(iencs))
    {
        int nEncs;
        iencs->getAxes(&nEncs);
        yarp::sig::Vector encs(nEncs);
        iencs->getEncoders(encs.data());

        yarp::sig::Vector joints;
        finger.getChainJoints(encs,joints);
        yarp::sig::Matrix tipFrame=finger.getH((M_PI/180.0)*joints);
    }
}

The helper function simply takes the encoders as input, extracts relevant components referring to the correct finger (the index in this case), and applies suitable transformations to them.

The resulting joints can then be used to populate the kinematic chain straightaway. Notice the conversion from degrees to radians.

[pattacini]

The /icub/left_arm/state:o port streams out only one double accounting for the movements of the two underactuated distal joints of the fingers. Moreover, the angles provided by this port do refer to motors encoders.

However, relying on the measurements acquired from the MAIS boards, we can have access to the two "hidden" encoders of each distal phalanxes.

The MAIS feedback can be profitably used to improve the accuracy of the forward kinematics computation also because they account for joints encoders that do sense what motor encoders cannot (e.g. tendon flexibility). We can thus specialize the snippet above for e.g. the left index as in the following:

yarp::os::Property options2;
options2.put("device","analogsensorclient");
options2.put("remote","/icub/left_hand/analog:o");
options2.put("local","/local/left_hand");

yarp::dev::PolyDriver driver2;
if (driver2.open(options2))
{
    yarp::dev::IAnalogSensor *ianalog;
    if (driver2.view(ianalog))
    {
        // analogs are expressed in the range 0...255
        // with reversed logic => we need conversion
        yarp::sig::Vector analogs;
        ianalog->read(analogs);

        // imagine you have available here the Vector joints
        // as prepared from the snippet above with resort to
        // iCub::iKin::getChainJoints() helper

        // get access to joints bounds
        iCub::iKin::iKinChain &chain=*finger.asChain();

        // apply corrections
        // joints[0] is the abduction/adduction for which we don't have any MAIS readout
        for (int i=1; i<=3; i++) {
            joints[i]=((1.0-std::min(1.0,std::max(0.0,analogs[2+i]/255.0)))*
                       (chain[i].getMax()-chain[i].getMin())+chain[i].getMin())*(180.0/M_PI);
        }
        yarp::sig::Matrix tipFrame=finger.getH((M_PI/180.0)*joints);
    }
}

The proximal and distal joints of the left index (the last 3 components) are corrected resorting to the analog readings, whereas we have still to use the motor encoder for the abduction/adduction movement (first component).

The IAnalogSensor interface wraps around the /icub/left_hand/analog:o port, providing a Vector of 15 numbers¹ in 0...255 (with reversed logic) that are arranged according to these two rules:

1. 3 numbers per finger accounting in order for the proximal and the two distal phalanxes.
2. the fingers' order is: thumb, index, middle, ring, pinkie.

References

1. Finger encoders

[pattacini]

Upon robotology/icub-main@23c474e it is now possible to streamline the above snippets in:

// imagine you have available here the Vector encs containing the motor encoders given in [deg]
// and the Vector analogs containing the joint encoders given in machine units

finger.getChainJoints(encs,analogs,joints);
yarp::sig::Matrix tipFrame=finger.getH((M_PI/180.0)*joints);

[pattacini]

It is likely that analogs readings are not covering the full range of motion of the fingers. In this case, the user can proceed by noting down the values of analogs when the fingers are fully extended and fully bent and construct the corresponding matrix of analogs bounds as below, with the first column accounting for full extension and the second for full bending:

yarp::sig::Matrix analogsBounds(15,2);
analogsBounds(0,0)=250.0; analogsBounds(0,1)=11.0; 
analogsBounds(1,0)=233.0; analogsBounds(1,1)=21.0;
// ... 
analogsBounds(14,0)=201.0; analogsBounds(14,1)=12.0;

finger.getChainJoints(encs,analogs,joints,analogsBounds);
yarp::sig::Matrix tipFrame=finger.getH((M_PI/180.0)*joints);

If the bound matrix is not provided, then a default table of analogs bounds is used with 255 and 0 values.

cc @giuliavezzani @claudiofantacci @vtikha @vicentepedro

[giuliavezzani]

Thanks @pattacini for the improvement! 😃
I tested the new function on the iCubDarmstadt01 and I got good positions both when I passed a analogBounds Matrix and when I used the default values in the getChainJoints(). 👍

However, it seems there is still some problems in getting fingers positions when the fingers are holding an object, thus, executing pressure on the object surface. I experienced this problem on the iCubDarmstadt01, whereas I don't remember any similar behaviour on the iCubGenova01 (but I should test it again).
If you are interested in the problem, you can find more info in this comment.