Clarification on the Hand MK5 coupling laws

[xEnVrE]

Hi all,

I am trying to implement the coupling laws available in https://icub-tech-iit.github.io/documentation/hands/hands_mk5_coupling/ to make some tests (more details in https://github.com/robotology/gazebo-yarp-plugins/issues/647).

Specifically, I would be interested in evaluating the expression of $q2$ as a function of the angle $q1$. That expression depends on $h(q_1)$ that in turn depends on $P_1(q_1)$. Both the $x$ and $y$ coordinates of $P1$ depends on additional constants, $P{0x}$ and $P_{0y}$ that I cannot find in the table comprising all parameters.

Are they missing or should I evaluate them using other available quantities given the geometry?

Thank you

cc @ale-git @mfussi66

[pattacini]

cc @Nicogene

[pattacini]

Hi @xEnVrE

Reading the documentation, it appears that $P_0$ is the origin of the coordinate system.

See for example the note below from the table:

Thereby, I deem we could assume $P{0x}=P{0y}=0$ without loss of generality[^1].

cc @ale-git

[^1]: This is reasonable as the proximal phalanx rotates around P0.

[xEnVrE]

Thanks for the clarification @pattacini.

I will try using the formula by assuming that they are both zero.

[xEnVrE]

I made some tests using a very simple python script that I am attaching to the comment.

I tested the index finger. The values that I got seem strange. For example:

$q1$	$q2$	$q2-q1$ (relative angle)
0.0	-166.66966836384188	-166.66966836384188
90.0	22.524220960903726	-67.47577903909627

When $q1 = 0$ I was expecting the relative angle to be $~0$. Instead, when $q1 = 90$ deg I was expecting the relative angle to be approx. $90$ as on the real finger:

I also made a plot:

It seems that the function is almost linear, but shifted down.

#

As a test, I tried re-evaluating the formula of $q2$ without the final term $-q{2bias}$, given the above consideration.

Results are:

$q1$	$q2$	$q2-q1$ (relative angle)
0.0	6.68033163615811	6.68033163615811
90.0	195.8742209609037	105.8742209609037

They seem more reasonable, however 105 degrees are probably too much considering the picture of the real finger. Moreover, removing $q_{2bias}$ is just arbitrary.

#

Here you can find the script. I double checked many times to see if there is any mistake, however I can check it once again.

test_coupling.py.zip

[ale-git]

Hi @xEnVrE, I was on vacation yesterday, I'm checking all the coupling stuff.

[xEnVrE]

Thanks @ale-git

[ale-git]

Hi @xEnVrE, I notice that in my original decoupler for Mk3 hand we have an expression for offset:

https://github.com/robotology/gazebo-yarp-plugins/blob/4b35f025c93aa51870c57e81d97fbcee273ed876/plugins/controlboard/src/ControlBoardDriverCoupling.cpp#L694

Then someone added the Mk4 and Mk5 handlers and replaced the expression with fixed values in them:

https://github.com/robotology/gazebo-yarp-plugins/blob/4b35f025c93aa51870c57e81d97fbcee273ed876/plugins/controlboard/src/ControlBoardDriverCoupling.cpp#L1222

Now I've made a little Matlab script (sorry, I'm not familiar with Python) to see if there is a difference in replacing the fixed expression with the fixed 173.35 value, and in fact there is.

With double offset = 173.35;

With double offset = RAD2DEG*atan2(L1y-P1y, L1x-P1x);

Notice that RAD2DEG*atan2(L1y-P1y, L1x-P1x) == -173.35 and not 173.35. Since we are in modular arithmetics mod 360 we have the 13.3 deg error you can see in the first plot.

[ale-git]

fingertester.zip

[xEnVrE]

Thanks for the clarifications and the MATLAB code @ale-git.

However, it seems that there are differences between the code you have provided and the documentation in this repository that I would like to clarify.

According to the documentation

When evaluating $h$ the documentation suggests:

$$ h(q_1) = ||P_1(q_1) - L_0||, $$

where $P_1(q1) = [P{1x}(q1), P{1y}(q_1)]$ and

$$ P_{1x}(q_1) = d cos(q1 + q{1off}), $$

$$ P_{1y}(q_1) = d sin(q1 + q{1off}), $$

where $d = || P_1 - P_0 ||$ is available in the table of the documentation . The above is what I implemented in the python script that I provided.

These two terms are also used to evaluate $arctan\left(\frac{P_{1y}(q1) - L{0y}}{P_{1x}(q1) - L{0x}}\right)$, that is required to evaluate $q_2$ as a function of $q_1$.

According to the MATLAB script

Instead, in the code you have provided it seems that the relationships are:

$$ h(q_1) = || P_1(q_1) - L_0 || $$

with

$$ P_{1x}(q_1) = cos(q1) P{1x} - sin(q1) P{1y} $$

$$ P_{1y}(q_1) = sin(q1) P{1x} + cos(q1) P{1y}. $$

where $P{1x}$ and $P{1y}$ are constants provided in the table of the documentation.

In summary:

• the term $q_{1off}$ is not used
• the formulation of $P_1(q_1)$ is different

Are these two formulations equivalent or one of the two is not correct?

Another difference that I noticed is in the term

$$ arctan\left(\frac{P_{1y}(q1) - L{0y}}{P_{1x}(q1) - L{0x}}\right) $$

that in the code is implemented as:

$$ arctan2\left(L{0y} - P{1y}(q1), L{0x} - P_{1x}(q_1)\right) $$

Given that the $arctan2$ takes into consideration the sign of the numerator and of the denominator separately, I am not sure it would return the same value as the following:

$$ arctan2\left(P_{1y}(q1) - L{0y}, P_{1x}(q1) - L{0x}\right) $$

which is what I would have implemented starting from the documentation.

Evaluation of the offset

Then someone added the Mk4 and Mk5 handlers and replaced the expression with fixed values

The same is suggested in the documentation:

where $q_{2bias}$ is constant and it is specified in the parameters table.

Thank you

[ale-git]

Hi @xEnVrE, about the first questions, i.e. the two implementations, they are equivalent since, at rest,

$P_{1y}(q1) = d sin(q{1off})$ $P_{1y}(q1) = d sin(q{1off})$

so you can pass from one form to the other by substituting cos(a+b) and sin(a+b) formulas. In the first case you calculate $P_1$ position from scratch and thus you have to use q1off; in the second case you rotate the original $P1$ at rest, that already includes $q{1off}$, by an angle $q_1$.

[ale-git]

Changing both parameter signs in atan2 adds Pi to the result, I'm going to check why they are inverted.

[xEnVrE]

Thanks for the clarifications, then the reasons for the differences between the plot you made and the one I obtained might depend on:

• the expression for evaluating q2_bias, that is the variable offset in your code;
• the way the arguments are passed to atan2;
• the necessity to use modular arithmetics.

I will test again my code with these changes to check that. Then I will also check if, after these changes, the value of the joint of the second phalanx is finally compatible with the picture of the real finger.

[xEnVrE]

I got the same plot as yours

in two different ways.

One way is:

• changing both parameter signs in $atan2$ and
• using $+q{2bias}$ instead of $-q{2bias}$ in the final expression of $q2$ and
• using modulo $2 \pi$ on the final result

The other way is:

• using $+q{2bias} - \pi$ instead of $-q{2bias}$ in the final expression of $q2$

The table with the values at the extremes become:

$q1$	$q2$	$q2-q1$ (relative angle)
0.0	0.030331636158094078	0.030331636158094078
90.0	189.2242209609037	99.2242209609037

I will try to test it in Gazebo and compare with the real finger configurations.

[xEnVrE]

99.2

Btw, this is also the number that @Lawproto confirmed when I asked for the limits of the second phalanx according to the CAD.

[pattacini]

After a T2T alignment w/ @xEnVrE, we decided to put this task on hold awaiting further tests on the physical robot, before updating the documentation.

cc @ale-git @Nicogene

[Lawproto]

I think it is worth checking that the coordinates of the points used for the formula are consistent with the CAD model - at least in zero position.

cc @xEnVrE @pattacini @ale-git

[Lawproto]

Posting a sketch useful to make a parallel between CAD and coupling laws documentation I did with @xEnVrE .

We verified that for the middle finger $q{0off} = 7.54°$, $q{1off} = 2.86°$ and $q_{2bias} = 173.35°$ (absolute values).

The next step is to check the length of the links.

[Nicogene]

I think it would be nice to have these drawing in the documentation

cc @pattacini

[pattacini]

I think it would be nice to have these drawing in the documentation

cc @pattacini

Agreed 👍🏻

[ale-git]

Hi @xEnVrE, about the atan sign, the documentation is fomally correct because the atan function codomain is in the (-Pi/2, Pi/2) interval, but the result must be taken carefully because it is correct only in mod Pi sense, so the implementation makes use of atan2 in order to avoid this ambiguity. The order inside atan in the documentation is irrilevant since the two minus signs in denominator and numerator cancel each other, while it is taken into account by atan2 in the implementation.

[xEnVrE]

The order inside atan in the documentation is irrilevant since the two minus signs in denominator and numerator cancel each other, while it is taken into account by atan2 in the implementation.

Yep, I agree

but the result must be taken carefully

For that reason, I think - although not sure if possible - that the documentation page should also include/attach source code implementing the laws so that the user can use them as a starting point. Having them implemented only within the Gazebo YARP plugins seems strange.

[pattacini]

Hi @Nicogene

We should report here the outcome of the meetup in terms of action points and who's required to do what.

[Nicogene]

Yesterday we had a meeting about this topic and here is the outcome. Participants: @xEnVrE, @valegagge, @MrAndrea, @ale-git, @Nicogene, @Lawproto

Action points:

• The gazebo-yarp-plugin coupling handler takes into account that the controlled joint is q2, but the firmware of the hands uses q1 instead. The idea is to align the simulator in order to behave like the robot. @xEnVrE will take care of changing the plugin and inform the users about this change. Moreover, we agreed that the fix in the equations proposed here should be correct
• The @xEnVrE's implementation (in draft) that considers q1 as a joint to be controlled seems to have flaws in the pinky dynamics. @xEnVrE and @Lawproto will check in the CAD if the parameters used in the coupling equations are correct for the pinky.
• This issue brought out once more the necessity of having a sw layer for joint coupling handling https://github.com/orgs/robotology/discussions/609. @icub-tech-iit/fix will take care of it as activity.
• Update the documentation with the coupling law fixed (@Nicogene)

[xEnVrE]

The @xEnVrE's implementation (in draft) that considers q1 as a joint to be controlled seems to have flaws in the pinky dynamics. @xEnVrE and @Lawproto will check in the CAD if the parameters used in the coupling equations are correct for the pinky.

Let me update the current status of our comparisons between the CAD and the formula using the parameters from the documentation - the following is limited to the range spanned by each joint:

finger name	$q1$ (range - expected from CAD)	$q2-q1$ (range - expected from CAD)	$q2-q1$ (range- from the formula)
thumb	0 -> 82.1	0 -> 53.6	2.6 -> 53.8
index/middle/ring	0 -> 90.0	0 -> 99.2	0.03 -> 99.2
little	0 -> 90.0	0 -> 93.3	6.6 -> 99.2

We will continue with the check of all the parameters next days w/ @Lawproto.

[Lawproto]

Little finger notes

@xEnVrE and I found out that the highlighted angle is quite off in the doc table.

[xEnVrE]

Taking into account the above, i.e. $q_{1off} = 3.43$ instead of $6.03$, the updated table is:

finger name	$q1$ (range - expected from CAD)	$q2-q1$ (range - expected from CAD)	$q2-q1$ (range- from the formula)
thumb	0 -> 82.1	0 -> 53.6	2.6 -> 53.8
index/middle/ring	0 -> 90.0	0 -> 99.2	0.03 -> 99.2
little	0 -> 90.0	0 -> 93.3	-0.06 -> 93.3

Hence, the little is now better in terms of the maximum limit and the minimum, that we expect to be 0, moved from 6.6 to -0.06.

[Lawproto]

Thumb notes

The remaining angle is 180° because the segment is parallel to the horizontal. Everything else we checked as absolute values seems right, which leaves us wonder why we still have >2° of error in $q_1 = 0$. Other things worth checking:

• The sign of the x and y components of the vectors.
• If the different shape of the "joining-points" polygon for the thumb has some effect on the formula (i.e.: it is not valid anymore or some pieces have to be changed in sign).

[xEnVrE]

I spotted I small bug in the verification code - I had set L0x = -5.50 mm instead of L0x = -5.55 mm as per the documentation. With this change, the table becomes:

finger name	$q1$ (range - expected from CAD)	$q2-q1$ (range - expected from CAD)	$q2-q1$ (range- from the formula)
thumb	0 -> 82.1	0 -> 53.6	-0.68 -> 53.6
index/middle/ring	0 -> 90.0	0 -> 99.2	0.03 -> 99.2
little	0 -> 90.0	0 -> 93.3	-0.06 -> 93.3

Hence, I could say that we are more or less satisfied also with the thumb now @Lawproto.

[xEnVrE]

At the moment it seems that there is nothing more that could be done to improve the lower bound obtained with the formula - if not checking if the actual expression of the formula is correct with respect to the drawings.

Should we proceed by opening a PR in order to correct the wrong parameter, as per https://github.com/icub-tech-iit/documentation/issues/257#issuecomment-1490258578, and the expression, as per https://github.com/icub-tech-iit/documentation/issues/257#issuecomment-1480291025?

Later on I could proceed with the fixes required to handle https://github.com/robotology/gazebo-yarp-plugins/issues/647 starting from the updated documentation.

Thanks

[pattacini]

Yep, better off proceeding incrementally. Thanks!