The HapticGlove coupling matrix regression can silently fail causing the retargeting to not work

[S-Dafarra]

We recently noticed that sometimes the retargeting does not start on one hand.

Checking the log, we noticed that the corresponding learnt coupling matrices contained nan

28860	4781,924201				RobotController::RightHand:: [axes-joints coupling] Bias-A matrix:

28861	4781,924221				        -nan         -nan         -nan         -nan         -nan         -nan   1.6839e-14 -3.24393e-15 -5.14172e-15

28862	4781,924253				        -nan         -nan         -nan         -nan         -nan         -nan -9.47293e-05  0.000150101  0.000174336

28863	4781,924278				        -nan         -nan         -nan         -nan         -nan         -nan  3.77637e-06 -3.20999e-06 -9.53803e-06

28864	4781,934221				        -nan         -nan         -nan         -nan         -nan         -nan            0            0            0

28865	4781,944277				        -nan         -nan         -nan         -nan         -nan         -nan -0.000181851 -1.43075e-05 -7.10659e-06

28866	4781,944302				        -nan         -nan         -nan         -nan         -nan         -nan            0            0            0

28867	4781,944315				        -nan         -nan         -nan         -nan         -nan         -nan            0            0            0

28868	4781,944327				        -nan         -nan         -nan         -nan         -nan         -nan      1.00008 -3.99134e-05   9.0291e-06

28869	4781,944344				        -nan         -nan         -nan         -nan         -nan         -nan    -0.225956     0.371033  -0.00728537

28870	4781,954345				        -nan         -nan         -nan         -nan         -nan         -nan     0.225943     0.477995   0.00723945

28871	4781,954390				        -nan         -nan         -nan         -nan         -nan         -nan   0.00176532    0.0119013     0.337929

28872	4781,954435				        -nan         -nan         -nan         -nan         -nan         -nan   -0.0173964   -0.0204783     0.418089

28873	4781,964412				        -nan         -nan         -nan         -nan         -nan         -nan   0.00629571   0.00288404     0.226472

28874	4781,964451				        -nan         -nan         -nan         -nan         -nan         -nan   0.00327127    0.0106662     0.303892

28875	4781,964474				        -nan         -nan         -nan         -nan         -nan         -nan  -0.00992028   -0.0174625     0.270478

28876	4781,964497				        -nan         -nan         -nan         -nan         -nan         -nan   0.00547984    0.0047442     0.267673

28877	4781,964520				RobotController::RightHand:: [axes-joints coupling] A (coupling) matrix:

28878	4781,974469				        -nan         -nan         -nan         -nan         -nan   1.6839e-14 -3.24393e-15 -5.14172e-15

28879	4781,974497				        -nan         -nan         -nan         -nan         -nan -9.47293e-05  0.000150101  0.000174336

28880	4781,974515				        -nan         -nan         -nan         -nan         -nan  3.77637e-06 -3.20999e-06 -9.53803e-06

28881	4781,974532				        -nan         -nan         -nan         -nan         -nan            0            0            0

28882	4781,974546				        -nan         -nan         -nan         -nan         -nan -0.000181851 -1.43075e-05 -7.10659e-06

28883	4781,974565				        -nan         -nan         -nan         -nan         -nan            0            0            0

28885	4781,974649				        -nan         -nan         -nan         -nan         -nan            0            0            0

28886	4781,984538				        -nan         -nan         -nan         -nan         -nan      1.00008 -3.99134e-05   9.0291e-06

28887	4781,984570				        -nan         -nan         -nan         -nan         -nan    -0.225956     0.371033  -0.00728537

28888	4781,984591				        -nan         -nan         -nan         -nan         -nan     0.225943     0.477995   0.00723945

28889	4781,984613				        -nan         -nan         -nan         -nan         -nan   0.00176532    0.0119013     0.337929

28890	4781,994607				        -nan         -nan         -nan         -nan         -nan   -0.0173964   -0.0204783     0.418089

28891	4781,994645				        -nan         -nan         -nan         -nan         -nan   0.00629571   0.00288404     0.226472

28892	4781,994668				        -nan         -nan         -nan         -nan         -nan   0.00327127    0.0106662     0.303892

28893	4781,994690				        -nan         -nan         -nan         -nan         -nan  -0.00992028   -0.0174625     0.270478

28894	4781,994712				        -nan         -nan         -nan         -nan         -nan   0.00547984    0.0047442     0.267673

28895	4782,004669				RobotController::RightHand:: [axes-joints coupling] B (bias) vector:

28896	4782,004705				-nan

28897	4782,004717				-nan

28898	4782,004727				-nan

28899	4782,004737				-nan

28900	4782,014737				-nan

28901	4782,014767				-nan

28902	4782,014783				-nan

28903	4782,014809				-nan

28904	4782,014820				-nan

28905	4782,024805				-nan

28906	4782,024834				-nan

28907	4782,024839				-nan

28908	4782,024845				-nan

28909	4782,024849				-nan

28910	4782,024855				-nan

28911	4782,034877				-nan

28912	4782,034915				 QP control coefficient matrix:

28913	4782,034928				-nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan

28914	4782,034949				-nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan

28915	4782,034968				-nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan

28916	4782,034982				-nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan

28917	4782,034997				-nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan

28918	4782,044936				-nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan

28919	4782,045010				-nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan

28920	4782,045031				-nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan -nan

Full log:
log_icub-head_HapticGloveModule_3407_nan.txt

[S-Dafarra]

By checking the log, it also appeared that rank of the input data matrix was 8, while it was 9 for the other (working) hand.

Indeed, if the input matrix is not full row rank, its inverse may fail:

walking-teleoperation/modules/HapticGlove_module/src/LinearRegression.cpp

Line 29 in 354e416

= ((inputData.transpose() * inputData).inverse()) * inputData.transpose(); // m X o

I suspect this was due to a joint not moving during the calibration phase. This might happen often if some finger goes to HF unnoticed. I would expect the module to print a meaningful error, or even better, to try to react ignoring the faulty joint.

cc @GiulioRomualdi @lrapetti @EhsanRanjbari