All functions handling rotations in input (except setWorldFrame) are broken about mex-wholebodymodel HOT 9 CLOSED

I will work on it. After this afternoon.

from mex-wholebodymodel.

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?

from mex-wholebodymodel.

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

from mex-wholebodymodel.

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 .

from mex-wholebodymodel.

Please check da6ceda

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

from mex-wholebodymodel.

@naveenoid I guess da6ceda solve the first point of the issue, but not the second ( Change all the code that is assuming the wrong behavior). @gabrielenava

from mex-wholebodymodel.

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.

from mex-wholebodymodel.

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.

from mex-wholebodymodel.

I am closing this for now since its completely addresses.

from mex-wholebodymodel.