Closed traversaro closed 8 years ago
I will work on it. After this afternoon.
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?
Just to understand better : The fix using reshape( ) in the relevant wbm_ wrappers is insufficient? Or is it incorrect?
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::Rotation
is 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);
Please check da6ceda10140ef6cec9f69133e32d514eb75e3ff
All tests are passing. ( @traversaro, @DanielePucci , @gabrielenava)
@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
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.
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.
I am closing this for now since its completely addresses.
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:
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.