torqueBalancing not working

[DanielePucci]

When launching mex-wholebodymodel based torqueBalancing, I get the following error:

Deleting older version of robotmexWholeBodyModel started with robot : icubGazeboSim, Num of Joints : 0 
Robot name set as 
Error using mexWholeBodyModel
Malformed state dimensions / inputs

Error in wbm_updateState (line 17)
        mexWholeBodyModel('update-state',varargin{1}, varargin{2}, varargin{3});

Error in initForwardDynamics (line 40)
wbm_updateState(qjInit,dqjInit,[v_bInit;w_omega_bInit]);

Error in initializeTorqueBalancing (line 123)
initForwardDynamics(CONFIG);

@gabrielenava @S-Dafarra @francesco-romano @traversaro

[gabrielenava]

This is probably due to a missing update of master branch after a change in its dependencies. I will update it today

[gabrielenava]

I think I found the cause of this issue. As we can see in the error log, the number of joints that are loaded by the model is zero (this then caused the malformed state/dimensions error):

mexWholeBodyModel started with robot : icubGazeboSim, Num of Joints : 0

I checked the function modelstate.cpp, that is in charge of reading the data from urdf. Apparently, getDoFs() (line 312) is returning zero:

https://github.com/robotology/mex-wholebodymodel/blob/master/mex-wholebodymodel/library/src/modelstate.cpp#L312 numDof = robotWBIModel->getDoFs();

robotWBIModel is a wbi::iWholeBodyModel class reference. I checked here the definition of getDofs, and it turns out that it is implemented in the classes yarpWbi::yarpWholeBodyModelV1, yarpWbi::yarpWholeBodyModelV2, and yarpWbi::yarpWholeBodyInterface.

In particular, i noticed that in yarpWbi::yarpWholeBodyModelV2 the definition of the function states (line 244):

int yarpWholeBodyModelV2::getDoFs() { return dof; }

so, it is returning the variable dof, but it seems to me that this variable is just initialized to zero at the beginning (line 65):

dof(0)

Instead, in the previous version yarpWbi::yarpWholeBodyModelV1, the variable dof is also called at line 126:

dof = jointIdList.size();

the interface has been changed a couple of moths ago, so this may be the reason why mex-wholebodymodel is not working anymore.

I will check now if my diagnosis is correct by adding the missing line in yarpWbi::yarpWholeBodyModelV2

cc: @DanielePucci @traversaro @francesco-romano

[DanielePucci]

👍

[traversaro]

I can confirm that this is the problem. @gabrielenava please remove the dof variable completely from yarpWholeBodyModelV2, as it is not necessary at all.

[gabrielenava]

fixed in this pull request: https://github.com/robotology/yarp-wholebodyinterface/pull/66 tested locally, the controller in now working properly.

[DanielePucci]

Nice guys 👍