KDLtoYarp_twist and KDLtoYarp_wrench conversion functions broken

[traversaro]

No description provided.

[traversaro]

For some reason , the KDLtoYarp_twist and KDLtoYarp_wrench methods are currently assuming a angular-linear serialization of the twist and wrench vectors , differently from the rest of the code base (including the others KDLtoYarp) in which the linear-angular is used.

A test ( cd248c6 ) was added to highlight this problem:

 Start 48: test_AdjointMatricesConsistency

48: Test command: /home/traversaro/src/codyco-superbuild/build/libraries/iDynTree/src/icub/tests/AdjointMatricesConsistencyTest
48: Test timeout computed to be: 1500
48: Adjoint matrix: 
48: iDynTree : 
48: 0.474387 -0.601891 0.642405 -0.35275 -1.12776 -0.796146 
48: 0.486925 0.787353 0.378125 1.47601 -0.970114 0.119312 
48: -0.73339 0.133425 0.666586 0.751806 0.637316 0.699584 
48: 0 0 0 0.474387 -0.601891 0.642405 
48: 0 0 0 0.486925 0.787353 0.378125 
48: 0 0 0 -0.73339 0.133425 0.666586 
48: 
48: YARP     : 
48: 0.474387 -0.601891 0.642405 0 0 0 
48: 0.486925 0.787353 0.378125 0 0 0 
48: -0.73339 0.133425 0.666586 0 0 0 
48: -0.35275 -1.12776 -0.796146 0.474387 -0.601891 0.642405 
48: 1.47601 -0.970114 0.119312 0.486925 0.787353 0.378125 
48: 0.751806 0.637316 0.699584 -0.73339 0.133425 0.666586 
48: 
48: Eigen    : 
48: 0.474387 -0.601891 0.642405 -0.35275 -1.12776 -0.796146 
48: 0.486925 0.787353 0.378125 1.47601 -0.970114 0.119312 
48: -0.73339 0.133425 0.666586 0.751806 0.637316 0.699584 
48: 0 0 0 0.474387 -0.601891 0.642405 
48: 0 0 0 0.486925 0.787353 0.378125 
48: 0 0 0 -0.73339 0.133425 0.666586 
48: 
48: Wrench Adjoint matrix: 
48: iDynTree : 
48: 0.474387 -0.601891 0.642405 0 0 0 
48: 0.486925 0.787353 0.378125 0 0 0 
48: -0.73339 0.133425 0.666586 0 0 0 
48: -0.35275 -1.12776 -0.796146 0.474387 -0.601891 0.642405 
48: 1.47601 -0.970114 0.119312 0.486925 0.787353 0.378125 
48: 0.751806 0.637316 0.699584 -0.73339 0.133425 0.666586 
48: 
48: YARP     : 
48: 0.474387 -0.601891 0.642405 -0.35275 -1.12776 -0.796146 
48: 0.486925 0.787353 0.378125 1.47601 -0.970114 0.119312 
48: -0.73339 0.133425 0.666586 0.751806 0.637316 0.699584 
48: 0 0 0 0.474387 -0.601891 0.642405 
48: 0 0 0 0.486925 0.787353 0.378125 
48: 0 0 0 -0.73339 0.133425 0.666586 
48: 
48: Eigen    : 
48: 0.474387 -0.601891 0.642405 0 0 0 
48: 0.486925 0.787353 0.378125 0 0 0 
48: -0.73339 0.133425 0.666586 0 0 0 
48: -0.35275 -1.12776 -0.796146 0.474387 -0.601891 0.642405 
48: 1.47601 -0.970114 0.119312 0.486925 0.787353 0.378125 
48: 0.751806 0.637316 0.699584 -0.73339 0.133425 0.666586 

[traversaro]

For some reason, everything that was involved in the external wrench estimation in the TorqueEstimationTree class was using angular-linear serialization in place of the linear-angular used everywhere else in the code. This was fixed in #124 , and the fact that the behaviour was not changed was verified on the real robot (in a scenario similar to the one described here: robotology-legacy/codyco-modules#160).