iCub Eye - How to get eye pose from encoder values

[claudiofantacci]

Hi everyone,
this a small, self-contained, "tutorial" on how to jointly use iCub joint values with iCubEye class to get, either left or right one, eye pose ✏️

Assume that you store/read encoder values from the iCub torso and head through either

• /icub/torso/state:o and /icub/head/state:o
• /icub/torso/stateExt:o and /icub/head/stateExt:o
• IEncoders class

you will find yourself with 3 values for the torso and 6 values for the head 🤔

According to the iCub reference frames (click on the following image for details)

you have 3 joints for the torso, and three for the neck.
So what are the other 3 values found reading head encoders? 🤔
They are, of course, related to the eyes!

Let's see what they are: vergence, version and disparity!
The following image depicts version (Vs) and vergence (Vg) which are of use for our purposes.

To get the an eye pose, we need to feed the iCubEye interface with the encoders from the ROOT to the desired eye, plus the L or R value (see previous picture) if you either need the left or the right eye pose.
As a result, the values to be passed to iCubEye are:

• [1-3]: 3 encoders from the torso (the torso encoders from /icub/torso/state:o)
  ⚠️ Beware: the torso joints must be stored in reversed order, from (Yaw, Roll, Pitch) -> (Pitch, Roll, Yaw).
• [4-6]: 3 encoders from the neck (the first three values from /icub/head/state:o)
• [7]: 1 encoder for the eye tilt (the fourth value from /icub/head/state:o)
• [8]: L|R value for the left|right eye (which is to be evaluated from the fifth and sixth values of /icub/head/state:o)

The L|R value can be evaluated from the version and vergence following the linear relationship reported here:

L = Vs + Vg/2;
R = Vs - Vg/2;

We now have all the values we need! We are ready to use iCubEye class to get the desired eye pose.
Here is a (almost) ready-to-use code snippet:

Vector q(8);
...
/* Store the 8 encoder values described before in q. */
...
iCubEye icub_kin_eye("left_v2");
icub_kin_eye.setAllConstraints(false);
icub_kin_eye.releaseLink(0);
icub_kin_eye.releaseLink(1);
icub_kin_eye.releaseLink(2);
Vector eye_pose = icub_kin_eye.EndEffPose(CTRL_DEG2RAD * q);

Let's now dig into the detail!

First and foremost: iCubEye class is fully documented here ✌️

setAllConstraints(false)

releases the constraint of all chain links so that, even if any of the encoder goes beyond the default encoder constrained value, the encoders are not saturated to their constraints. If you feel uncomfortable with this, you can use alignJointsBounds member function to properly set the encoder limits.

releaseLink([0|1|2])

releases the first three links of the chain from the ROOT to the eye, i.e. the 3 DOF of the torso. By doing so, we are telling the interface to use the first 3 values of vector q
as torso encoders.
⚠️ Beware: by not releasing the first three links while having used them, will most likely produce wrong/unwanted results!

EndEffPose(CTRL_DEG2RAD * q)

finally gives us the pose of the eye. The output vector is, by default, a 7x1 vector with xyz coordinates plus a 4-component axis/angle vector. If you prefer RPY angles, you just need to provide false as a second argument. Documentation of the member function can be found here.
⚠️ Beware: a common pitfall is to forget to convert the encoder values, expressed in angle degrees, in radians multiplying by CTRL_DEG2RAD.

If you went up through here, well, congratulations 🎉 you now have the eye pose at your preferred use!

[vicentepedro]

Hi Claudio!

Thanks for this "tutorial" ;)

I have an issue related to the torso joints update.

Usually, when I use the iCubArm chain (similar to the iCubEye, I suppose), I need to update the torso joints in the reverse order.

armKin.releaseLink(0);
armKin.releaseLink(1);
armKin.releaseLink(2);
armKin.blockLink(0,CTRL_DEG2RAD*encodersTorso[2]);
armKin.blockLink(1,CTRL_DEG2RAD*encodersTorso[1]);
armKin.blockLink(2,CTRL_DEG2RAD*encodersTorso[0]);

Can you confirm that for the iCubEye we don't need to reverse the joint order?

Thanks in advance,
Pedro

[claudiofantacci]

Hi @vicentepedro :-)

For "historical" reasons, the iCub torso joints are reversed, see here, i.e., (Yaw, Roll, Pitch).
As a result, you read the encoders and then you have to flip them from (Yaw, Roll, Pitch) -> (Pitch, Roll, Yaw).
I updated the tutorial accordingly.

Hope it helps!

[vicentepedro]

Thanks, @claudiofantacci. Now, I think the tutorial it's perfect.

Hope it helps the iCub community ;)

[claudiofantacci]

Cool, thanks @vicentepedro !

[barbalberto]

👍

[pattacini]

Interestingly, the same recipe is also valid to correctly populate other iKin classes, such as iCubArm or iCubTorso.

[claudiofantacci]

Should I open another QA, say,
iCub iKin - How to get end effector pose from encoder values
with a phrase like yours @pattacini which, however, simply points here ?

[pattacini]

@claudiofantacci I don't believe it's really necessary. Later searches through GitHub will catch your nice instructions anyway 😉