All functions handling rotations in input (except setWorldFrame) are broken

[traversaro]

See https://github.com/robotology/mex-wholebodymodel/pull/57/files#r44255064 .

TL;DR : the rotation matrix in input is not properly converted from column major storage order (Matlab) to row major storage order (WBI).

Fix:

• [x] propagate the fix in https://github.com/robotology/mex-wholebodymodel/blob/efc77a2feb1b8ec13f9693770cf3c1ce07cc2f1a/mex-wholebodymodel/library/src/modelsetworldframe.cpp#L88 to all the functions handling rotations. To avoid repeating mistakes like that, consider to implement that conversion in a function that is used by all the components (see https://github.com/robotology/mex-wholebodymodel/issues/42 ).
• [x] Change all the code that is assuming the wrong behavior.

I think it make sense that we do the fix directly in the https://github.com/robotology/mex-wholebodymodel/tree/controllers_update branch.

@naveenoid @gabrielenava Do yo think you can solve this in Genoa before Wednesday ? If you have other priorities just let me know, so I can plan to work on this myself.

[naveenoid]

I will work on it. After this afternoon.

[DanielePucci]

Hi guys, this is of fundamental importance. We are currently mounting the inverse kinematics in Simulink, and we need dynamic quantities evaluated at the desired configurations. On one hand, some simulink blocks (e.g. jacobians) cannot be evaluated in other points than the state, since they do not take it as input @jeljaik. On the other hand, this bug forbids tests of an mex-wholebody model implementation.

May we try to address it ASAP?

[naveenoid]

Just to understand better : The fix using reshape( ) in the relevant wbm_ wrappers is insufficient? Or is it incorrect?

[traversaro]

As far I can see, reshape is simply mapping the n x n matrix to a n^2 double vector using a column-major ordering (so you get a vector similar to how the matrix is actually displayed in memory:

>> A = [1,2;3,4]   

A =

     1     2
     3     4

>> reshape(A,[],1)

ans =

     1
     3
     2
     4

However, the data in the wbi::Rotationis stored using the classical C-Style row major ordering, i.e. in memory the data is stored as :

double mat[2][2];
mat[0][0] = 1.0;
mat[0][1] = 2.0;
mat[1][0] = 3.0;
mat[1][1] = 4.0;

double * p_mat = mat;
assert(p_mat[0] == 1.0);
assert(p_mat[1] == 2.0);
assert(p_mat[2] == 3.0);
assert(p_mat[3] == 4.0);

See https://en.wikipedia.org/wiki/Row-major_order .

[naveenoid]

Please check da6ceda10140ef6cec9f69133e32d514eb75e3ff

All tests are passing. ( @traversaro, @DanielePucci , @gabrielenava)

[traversaro]

@naveenoid I guess https://github.com/robotology/mex-wholebodymodel/commit/da6ceda10140ef6cec9f69133e32d514eb75e3ff solve the first point of the issue, but not the second ( Change all the code that is assuming the wrong behavior). @gabrielenava

[naveenoid]

yes thats right. I can check the @gabrielenava code if one of you gives me a quick overview. I can help maintain it from now on.

[gabrielenava]

I fixed the original and the joint space balancing controller. I don't think there are other functions that assume the wrong behaviour. I also tested the balancing and it seems that everything is working fine.

[naveenoid]

I am closing this for now since its completely addresses.