robot Jacobian from left-eye to right-hand using iKin

[vicentepedro]

Hi all,

I need the robot Jacobian from the Cartesian reference frame centered in the left eye to the Cartesian reference frame centered in the right hand, in order to define the following equation:

where q are the joints of the arm and head, and s is the hand pose in the left eye reference frame.

I was wondering the best way to do it.
I see two possibilities right now:

1. Defining a new kinematic structure using iKin, from the left-eye to the right hand
2. somehow use the iCubEye and iCubArm class definitions to achieve the new kinematic chain

Maybe @pattacini ?

Thank you,

Pedro Vicente

[pattacini]

Hi @vicentepedro

I'd suggest you to proceed with solution (1), being careful about the way you'll be defining the part of the chain going from the left-eye to the neck (you don't need to go through the torso indeed). In fact, you are required to sort of "revert" the DH kinematics parameters.

@alecive already studied this problem and proposed the simple transformation described in Roncone A. et al. 2014.

Given the parameters of the direct chain from the neck to the left-eye, you can easily determine the parameters of reverse chain as per the following formula:

and then complete the chain with the remaining direct parameters from the neck to the right-hand.

Once done with that, you can call the iKin::iKinChain::GeoJacobian() method.

[alecive]

@vicentepedro I also already implemented a library that takes into account this problem as well. It is an extension of iKin, and it is available here -> https://github.com/alecive/periPersonalSpace

You would be the first "external" user, so it is not that documented, but in short you would have to do the following :

• Allocate the first part of the chain (from the eye to the neck base) as an inverted link (i.e. a iKinInvertedLink)
• Compute a fixed rototranslation matrix from the base of the neck to the base of the shoulder (easily computable from the chains already available) and add that as an iKinFixedRTLink
• Add the remaining part of the chain, i.e. the arm (and eventually the fingers), as usual (i.e. by pushing them to the chain as iKinDirectLink).

For further information, see https://github.com/alecive/periPersonalSpace/blob/master/lib/src/iKinFwdMod.cpp#L1569 : in my case, I needed four custom chains. Recalling the three steps listed above, for the chain initialized as LHtoR (left hand to right arm), I had to do the following:

• Allocate the first part of the chain (from the left hand to the neck base) as an inverted link, line 1603
• Compute a fixed rototranslation matrix from the base of the left shoulder to the base of the right shoulder and add that as an iKinFixedRTLink, line 1606
• Add the remaining part of the chain, i.e. the right arm (and eventually the fingers), as usual, line 1609

The jacobian regarding the reverted part of the chain is slightly different but it is already taken care of inside my library, so after the creation of the chain you should simply call the standard GeoJacobian function.
I am available for feedback and support. Feel free to use it, and to suggest any improvement when you do!

[vicentepedro]

Hi @alecive,

Thanks for your reply. I already started changing the library to add my kinematic structures.

But I have another question, I saw in your code the fix RT matrix from the L shoulder to the R shoulder. In order to compute it you use these matrices defined here:

http://eris.liralab.it/images/9/97/NewiCubRefFrames1.png

Or you are using another method.

Thanks in advance

[alecive]

@vicentepedro when you have finished, feel free to pull request on my repo, I will be glad to add also your custom chains to the library!

Regarding the fixed RT matrix, I computed it by hand on matlab: that file was not available at the time, and I did not check if my matrix fits with that one. In your case (e.g. eyeToRightHand), your fixed RT matrix would be Hup^-1 * Hright^-1 according to that picture. Considering that this matrix is critical for the task, I would suggest you to compute it by hand, though (just to be sure that everything is fine).

Let me define this:

• H_TE -> transform matrix from the torso ROOT to the neck base (can be computed from iKin)
• H_TR -> transform matrix from the torso ROOT to the right shoulder base (can be computer from iKin)
• H_ER -> transform matrix from the neck base to the right shoulder base (this one is the one that you need)

Then, you have that:

H_TE * H_ER = H_TR

so:

H_ER = H_TE^-1 * H_TR

[pattacini]

closing; @vicentepedro feel free to open it again when you want.

[vicentepedro]

Thanks for your help, @pattacini.

@alecive, I will make a pull request in a near future to your repo, I will add also the LE to LH chain and the kinematic chain from the right eye to the hands. I will try to follow your notation, but feel free to change it afterwards. Thanks for your help.

Just a final note, I didn't use the Hup and Hright RT matrix. I followed your suggestion, and for now everything is working fine.