Closed traversaro closed 8 years ago
cc @naveenoid @gabrielenava
To make sure that everything is properly working, I suggest that we do some consistency tests together with wbm_setWorldFrame
,wbm_updateState
and wbm_forwardKinematics
.
Anyway the effect of the simulations is limited, as getState was used only to get the initial configuration of the base.
I don't think I have really understand this problem. I mean, in the balancing controller wbm_getState is used only at the beginning and only to obtain the identity since wbm_setWorldLink sets exactly the identity transform. But if I put wbm_getState in forwardDynamics just to see what happens the quaternion returned by getState changes, it's still a valid quaternion and the associated rotation matrix is not the identity so I don't think I understand this problem properly
I'll explain what I found in the code:
wbm_getState
calls (with some magic )
ModelGetState::computeFast that itself calls
modelState->getRootWorldRotoTranslation()
to get the rototranslation, but this function is not returning anything!
The fact that you mention wbm_setWorldLink
(that should not be used in simulation, see https://github.com/robotology/mex-wholebodymodel/issues/43) make me wonder if perhaps ModelGetState::compute
is called, and that function seems is not affected by the bug.
Anyway a general comment: the fact that a computed quantity changes and is not the defuault does not mean that it is correctly computed!
In particular, the base state was undefined because it was taken from the returned value of a function that was not returning anything (see https://github.com/robotology/mex-wholebodymodel/commit/0437e086ba93754111f219b1a86aae8113722958#commitcomment-13937981 ). I guess the function is always returning the identity transform in practice. Fixed in https://github.com/robotology/mex-wholebodymodel/commit/0437e086ba93754111f219b1a86aae8113722958 , still need to be fixed in master.