Function printBerdyDynVariables_floating.m does not print properly the variables

[claudia-lat]

This issue is imported from the previous repo on MAPest in claudia-lat profile. I'll copy paste here following the history of the issue.

claudia-lat commented on 25 May 2018

I found this wrong behaviour when dealing with the floating-base analysis. The function printBerdyDynVariables_floating.m is a MAPest function that prints the dynamics variables in the vector d, their position and their range in the vector itself, like this (for a model with 49 links, 48 joints):

In the case of the floating-base formalism, the vector d seems to have (from the above-list) the following order:
\begin{equation*}
d = [ a_1, f^x_1, a_2, f^x_2, \cdots, a_{N_B} f^x_{N_B},f_1,f_2, \cdots, f_{N_B} ]
\end{equation*}
where:

• is the 6D acceleration corresponding to the iDynTree.LINK_BODY_PROPER_CLASSICAL_ACCELERATION type,
• is the net external wrench corresponding to the iDynTree.NET_INT_AND_EXT_WRENCHES_ON_LINK_WITHOUT_GRAV type,
• is the joint wrench (force exchanged from two links through the joint) corresponding to iDynTree.JOINT_WRENCH type

As you can see from the picture, the vector contains 876 variables
( 49x6 + 49x6 + 48x6)
but if you compute the number of variables from the berdy model (via berdy.getNrOfDynamicVariables()) you obtain 924 that is exactly the dimension if you add the joint acceleration mapped from iDynTree.DOF_ACCELERATION type. But this function is not able to print
it.

[claudia-lat]

claudia-lat commented on 25 May 2018

The berdy model is launched with the following options:

berdyOptions = iDynTree.BerdyOptions;

berdyOptions.includeAllNetExternalWrenchesAsSensors          = true;
berdyOptions.includeAllNetExternalWrenchesAsDynamicVariables = true;
berdyOptions.includeAllJointAccelerationsAsSensors           = true;
berdyOptions.includeAllJointTorquesAsSensors                 = false;

berdyOptions.berdyVariant = iDynTree.BERDY_FLOATING_BASE;
berdyOptions.includeFixedBaseExternalWrench = false;

[claudia-lat]

traversaro commented on 25 May 2018

I think the problem is that these two lines are commented:
https://github.com/claudia-lat/MAPest/blob/test/floatingBase/src/printBerdyDynVariables_floating.m#L32 .

claudia-lat commented on 25 May 2018

It is commented because it was originally committed thinking that the d vector for the floating base didn't contain it (it is the only valid commit I had got to steer you in the problem). I did several tests with those lines uncommented but the problem persists.

traversaro commented on 25 May 2018
Can you add a default case to the switch in which an error such as: "Unexpected type in sensorsInfo " and then you print the type? In this way we can understand of which type are the variables that we are not printing.

claudia-lat commented on 25 May 2018
If you are saying this

switch(type) 
        case iDynTree.LINK_BODY_PROPER_CLASSICAL_ACCELERATION
            typeStr = '6D acc      ';
        case iDynTree.NET_INT_AND_EXT_WRENCHES_ON_LINK_WITHOUT_GRAV
            typeStr = 'Net Wrench  ';
        case iDynTree.JOINT_WRENCH
            typeStr= 'Joint Wrench';
        case iDynTree.DOF_TORQUE
            typeStr = 'Joint torque';
        case iDynTree.NET_EXT_WRENCH
            typeStr = 'Ext. Wrench ';
        case iDynTree.DOF_ACCELERATION
            typeStr = 'Joint acc   ';
        otherwise  
            typeStr = 'No var';
    end

I can confirm you the same situation of before.

traversaro commented on 25 May 2018

The code that populates that vector is https://github.com/robotology/idyntree/blob/78b413890dbe0afe624fc46a4deb57660b3ac783/src/estimation/src/BerdyHelper.cpp#L1767 .
I can't see any clear bug, but indeed the tests for that function are done only for the fixed base variant, see https://github.com/robotology/idyntree/blob/3fc1206dc5b8dfa5cd6b11f195ad5b7568ef771e/src/estimation/tests/BerdyHelperUnitTest.cpp#L304 . To be honest the whole test for fixed base apparently is disabled for some reason: https://github.com/robotology/idyntree/blob/3fc1206dc5b8dfa5cd6b11f195ad5b7568ef771e/src/estimation/tests/BerdyHelperUnitTest.cpp#L337 .

Link to the issue: robotology/idyntree#445

[claudia-lat]

traversaro mentioned this issue on 3 Oct 2019

Link to the issue: robotology/idyntree#587

lrapetti commented on 3 Oct 2019

Should be fixed by robotology/idyntree#587, the DOF acceleration was missing from the cached dynamics variable.
@claudia-lat can you test after the PR is merged?

[claudia-lat]

Tested. Now it works. After the list of the joint wrench now the function printBerdyDynVariables_floating(berdy) prints also the joint accelerations like in the following

Closing issue. Thanks @lrapetti