Approach method 2: consider first and second arm as a closed chain

[giuliavezzani]

This is to integrate Roncone's code in our scenario.

[giuliavezzani]

The idea is to integrate pose selection discussed in #15 and a close chain approach, as implemented here.

The close chain approach, in fact, must be used also during pose selection procedure. We need to take into account the fact that also the first hand moves in order to correctly select a good pose.

[giuliavezzani]

Short summary of the work accomplished in commit 4cad86e:

• I modified doubleTouch code so that to work with the palms of both hands, without using the index finger. I tested the behavior asking the robot to overlap its palms:

• The pose to be reached is directly sent by the pose-selection module, without waiting for any contact points on the robot arms

• In the tests I performed, I used the left arm as the slave and the right arm as the master and I asked the right hand to reach a pose expressed in the left hand reference system. However, I noticed that the reference system of left hand ( that is the origin of the close chain) is different from the standard hand reference frame: in particular, all the axes have opposite directions. In the following example, I asked the right hand to reach the position x= -0.2, y= 0, z= 0, expressed in the left hand frame:

and then, to reach the position x= 0.2, y= 0, z= 0, always in the left hand frame:

I performed similar tests also for the y and the z axis, and I noticed the same behaviour. We need to better investigate this fact (maybe on the related paper) and to take into account for the handover task.

[pattacini]

Good starting point @giuliavezzani 🎖

Perhaps, @alecive didn't investigate in depth this aspect in his previous work, 'cause he was concerned only with reaching the origin of the floating root reference frame, not a point expressed in that coordinates system. That's only my educated guess, although I might be wrong.

Anyway, I believe it should be pretty easy to get around this problem by using a suitable H0 matrix that conveniently rotates the axes of that floating root reference frame.

[giuliavezzani]

That's what exactly I'm doing now :smiley: But I need to investigate the problem in the detail, since I got some strange poses in trying to perform the handover task. There might be an error in my code or there might be something else to take into account :wink:

[giuliavezzani]

I performed further tests in order to better discover how to define the desired pose for the moving hand. The scenario is the following:

• end of the chain : right arm
• origin of the chain : left arm
• end effector of the moving arm: palm
• task: to reach the following positions (expressed in the left hand frame) with the right palm without asking any particular orientation for the right arm

position no. 1: x=-0.2 y=0.0 z=0.0

position no. 2: x=0.0 y=-0.2 z=0.0

position no. 3: x=0.0 y=0.0 z=-0.2

At first, I just thought that it was enough to apply a correction matrix H0, so that to invert the direction of all the axes. But if we do like that, the final frame is not a right-handed frame, and in fact, the H0 we would need is : H0=diag(-1, -1, -,1 ,1), whose determinant is equal to -1.

I'm a little bit confused about this result and I'll keep working on this problem, trying to understand, at first, if my approach and my code is correct and, then, if there is some problems in the closed chain library.

[giuliavezzani]

Problem in the previous comment has been solved :smile:

[pattacini]

Great news, does this mean it's now working nicely?

[giuliavezzani]

Not yet, I'm checking the operation in our scenario.

At the moment, if I ask the right palm to reach position no.1, 2 and 3 of the previous comment, I got the correct positions, withouth using the H0 matrix (with det= -1.0), mentioned before.

[giuliavezzani]

After a detailed study, I managed to make the code working :tada:

Brief summary

1. I had some problems with the reached orientations. Positions were correct (as said in the previous comment) but orientations were not consistent

2. I performed some tests, asking the right hand to reach particular and simple orientations (such as: x-right=x-left, y-right=y-left, z-right=z-left)

3. This way, I found the correction matrix to be applied:

   H_corr= [ 0  1  0
         0  0 -1
        -1  0  0 ]

4. I tested the use of H_corr successfully in the handover task

Results

The following figures shows the 8 poses in the estimated object pose and the iCub hands in the final positions (from the iCub cameras viewpoint, on the left and from an external viewpoint, on the right) .

Currently, the best pose is selected by using the standard iCub kinematics and enabling the torso. But this is just a method to select and compute a reachable pose. The pose obtained by inverting the kinematic is shown together with the 8 poses since it is not always exactly overlapped with the desired pose.

The final hand poses are instead computed and executed by the two-arms closed-chain.

• in this example, the selected pose is no. 1. Figure on the left shows how the closed-chain is correctly working.

• in the following example, the selected pose is no. 7 and the closed-chain allowed the robot to correctly¹ reach the desired pose.

¹ the orientation is not exactly the same, but we can accept this kind of results at the moment.

[pattacini]

Nice!

One major comment though: I wouldn't rank poses on the basis of one hand approach + torso. This has barely to do with the final overall movement indeed...

It should be fairly easy to use the new kinematics chain also during ranking stage.

[giuliavezzani]

Yes, of course! This is just a temporary result :smiley: Next step is to use the new kinematics chain also to select the best pose.

[pattacini]

Perfect!

[giuliavezzani]

Code has been updated in order to perform pose selection according to the new closed-chain kinematics.

Results

This is an example of pose ranking obtained with closed-chain:

According to the red-green colour scale, pose 2 and 7 are the best ones, since they are enough far away from the first hand and the orientations are easily reachable by the second hand. This raking turns out to be the same we obtained with the one hand approach. That's a good news, since pose 2 and 7 are actually the only good poses for the handover task, in this example.

NB: the hand poses shown in the figures are the final poses, after the movement has been already accomplished.

To-do

• code improvements (reliability, structure, interaction with user)

• I still have a problem with getting the correct pose of the first hand with iCubArm interface. I need it just for improving the visualization and adding some checks during pose selection, but that's not necessary for a correct pose selection.

[pattacini]

Superb! It is a great satisfaction when things behave as we had in mind when the project started! 😃

The only point we still need to take in consideration is that the first hand will be moving, hence it is not unlikely that the object might displace or even fall. To avoid that, we could make the movement slower, limit the displacement of the first hand and so on...

[giuliavezzani]

Yes, this is a quite good result :smile: This point you stressed out is absolutely correct and, unfortunately, is not the only critical aspect of our task. Let's keep working hard and let's hope to manage to solve all problems :muscle:

[giuliavezzani]

Problems ranked in the previous comment (to do section) have been solved (just debugging).

I also successfully checked that

• the modified Roncone's solver correctly computes the desired pose for the second hand:

• and the desired pose is reached with ~1 cm of error in position:

[giuliavezzani]

Issue summary: best pose selection with two-arms closed chain

This is to summarize my work on closed-chain approach for handover task.

Idea

Best pose for handover task is selected by using a two-arms closed chain. The poses attached to the object model are ranked according to 3 parameters:

• distance from the hand holding the object (first hand)
• manipulability index of the closed chain computed configuration
• errors between the desired poses and the poses computed by the inverse kinematics. Good poses are enumerated with greenish colours, whereas poses with bad scores are coloured in reddish.

Main upgrade

Previous approach: consider first hand fixed and solve the kinematics for the second hand, enabling torso (the second hand workspace is too small without enabling the torso). Cons: we should controll both the arms, keeping fixed the first hand and moving the second hand

Current and new approach: use a unique closed-chain including both the arms.

What I did:

• I modified doubleTouch code in order to accomplish the handover task, instead of making the iCub touching one of its arms
• I used the closed-chain in order to compute manipulability and actually perform the handover

Results

I performed some tests in order to test the correct working of the new approach.

Scenario:

• _firsthand: left
• _secondhand: right

Tests

• Pose ranking is consistent with the previous results. The best poses are no. 2 and 7, that are the only feasible poses for the handover task in such a scenario. In addition, the kinematics of the new closed-chain manages to perfectly solve the problem (very small errors in positions, acceptable in orientations). In the following picture, the first hand is in the initial position, whereas the 8 poses (helping us to understand the new first hand pose), the computed pose and the second hand are in the final desired poses:

• The closed-chain manages to reach the computed poses, with an accuracy of ~1cm:

To Do

• Code improvement
• More intensive tests (also with right arm as first hand)
• Design a approach strategy
• Tests on the robot