Errors when launching torqueControlBalancing

[DanielePucci]

I am trying to lunch the Yoga demo using the floating-base-balancing-torque-control.

After installing the robotlogy-superbuild thanks to the help of @traversaro - and during which process we found some problems due to the ARM on macOS https://github.com/robotology/robotology-superbuild/issues/1044#issuecomment-1893278003 - I was able to happily launch gazebo

Also, I was able to compile the floating-base-balancing-torque-control

But the problem arose when launching the module that threw the error

Error reported by S-function 'BlockFactory' in 'torqueControlBalancing/MOMENTUM BASED TORQUE CONTROL/Robot State and References/Compute base to fixed link transform/LFoot to base link transform /Fixed base to imu transform/S-Function':
Cannot find imu_frame in the frame list.
Component:Simulink | Category:Block error

I am trying to understand how to make it run anyways, but it seems that there is the lack of a frame.

@traversaro @Nicogene

[traversaro]

Thanks @DanielePucci, indeed this is due to https://github.com/robotology/icub-models/issues/230 . A quick workaround may be to change imu_frame to head_imu_0, as long as you are testing iCub 2.5 and not iCub 2.6 or 2.7 it should work fine.

fyi @gabrielenava

[DanielePucci]

Thanks @traversaro for the heads up. I modified the name of the frame at the line https://github.com/robotology/whole-body-controllers/blob/master/controllers/floating-base-balancing-torque-control/app/robots/iCubGazeboV2_5/configRobot.m#L37 and actually this solved the issue.

After this, however, I got the error

Error reported by S-function 'BlockFactory' in '[torqueControlBalancing/IMU_meas](matlab:open_and_hilite_hyperlink ('torqueControlBalancing/IMU_meas','error'))':
Failed to connect /icubSim/inertial to /tmp/port/1.  
Component:Simulink | Category:Block error

Thanks to @gabrielenava, this was solved by removing the "Error on missing connection" option in the read port block, which was actually on - see below

We should understand then why this happens (changing name of the port or format, etc).

After this, I was able to launch the yoga

https://github.com/robotology/whole-body-controllers/assets/5045846/b6a60f3e-1c80-49d2-9e25-97e6a439c9ef

There is the initial motion that is suspicious, but I think that the rest is fine. The robot cannot switch from one foot to the other, but I guess that this is a matter of tuning

[traversaro]

We should understand then why this happens (changing name of the port or format, etc).

Probably it is a consequence of deprecation and subsequent removal of the /icub/inertial port, see https://github.com/orgs/robotology/discussions/360 for a related announcement. Unfortunately it is not trivial to fix this as the IMU are now published with the multipleanalogsensors interfaces, and we do not have out of the box support for that in WB-Toolbox. Perhaps somebody in the iRonCub team may know some workaround around that. @gabrielenava

[DanielePucci]

I believe that I was kinda of lucky, since I was not able to reproduce consistently the demo in the comment above. In particular, I noticed that the Real Time Factor when running the controller does not go to zero

A feature that I remember that it should be there when controller and simulator are synched. Gazebo seems ok, since I made the alias in the .zshrc

alias gazebo="gazebo -slibgazebo_yarp_clock.so"

and in fact when I launch gazebo in the terminal, the first lines are

(robsub) dpucci@IITBMP014LM001 ~ % gazebo
[INFO] GazeboYarpClock loaded. Clock port will be  /clock
[INFO] |yarp.os.Port|/clock| Port /clock active at tcp://10.240.2.11:10002/
[INFO] |yarp.os.Port|/clock/rpc| Port /clock/rpc active at tcp://10.240.2.11:10003/
Resetting YARP clock to default

I also checked that I am using the synchroniser in the controller, and it seems so since my YARP_ROBOT_NAME is 'iCubGazeboV2_5' and the variable used for activating the synchroniser is set to true https://github.com/robotology/whole-body-controllers/blob/master/controllers/floating-base-balancing-torque-control/app/robots/iCubGazeboV2_5/configRobot.m#L6

[traversaro]

We should understand then why this happens (changing name of the port or format, etc).

Probably it is a consequence of deprecation and subsequent removal of the /icub/inertial port, see https://github.com/orgs/robotology/discussions/360 for a related announcement. Unfortunately it is not trivial to fix this as the IMU are now published with the multipleanalogsensors interfaces, and we do not have out of the box support for that in WB-Toolbox. Perhaps somebody in the iRonCub team may know some workaround around that. @gabrielenava

Other related issues:

• https://github.com/robotology/yarp/issues/1693
• https://github.com/robotology/robots-configuration/issues/462

[traversaro]

That "Resetting YARP clock to default" is fishy, I saw something similar in a issue by Guglielmo.

[DanielePucci]

To test if the possible synchronisation issue, I followed the @traversaro suggestion to check the RPC port

yarp rpc /clock/rpc

with start and stop commands, and it seems to work

https://github.com/robotology/whole-body-controllers/assets/5045846/5b41b020-68e1-49d7-95c4-3bccebf7fc1f

So probably the problem is on the "Simulink" side

[DanielePucci]

It seems that the problem above https://github.com/robotology/whole-body-controllers/issues/151#issuecomment-1893750712 related to the synchroniser was apparent since after sometime the simulation starts, the real time factor does go to zero - although not immediately. By talking to @traversaro, it may be a problem of how simulink computes the real time factor. So, I am closing this issue.