Synchronisation problems between Gazebo and estimation threads

[DanielePucci]

Description:
The Gazebo simulation and the estimation threads in charge of estimating joint position/velocities/etc are not synchronised even when using the appropriate block, and this introduces an undesired machine-dependence of the controllers performances.

@traversaro @francesco-romano

---

Further Information:

• System Information: Darwin Danieles-MacBook-Pro.local 15.5.0 Darwin Kernel Version 15.5.0: Tue Apr 19 18:36:36 PDT 2016; root:xnu-3248.50.21~8/RELEASE_X86_64 x86_64
• Working branch: master
• Current commit: cc53fb3c750899a0dddcae413a9484a6dfee24b4
• YARP ROOT: /Users/DanielePucci/src/yarp
• ICUB_ROOT: /Users/DanielePucci/src/icub-main
• YARP_DATA_DIRS: /Users/DanielePucci/src/install/share/yarp:/Users/DanielePucci/src/install/share/iCub:/Users/DanielePucci/src/codyco-superbuild/build/install/share/codyco:/Users/DanielePucci/src/install/share/ICUBcontrib
• YARP_ROBOT_NAME: iCubGenova05
• Yarp Installation: /Users/DanielePucci/src/install/bin/yarpserver
• LD_LIBRARY_PATH:``
• LD_PRELOAD:``
• DYLD_INSERT_LIBRARIES:``

[traversaro]

The /clock port is still published by the gazebo_yarp_clock plugin, even if the simulation is paused and stepped by the simulink gazebo synchronization block.
Starting MATLAB prepending the matlab command with YARP_CLOCK=/clock should solve the issue, however I don't know if there is any issue with that with our current workflow.
@DanielePucci suggested using setenv to set the YARP_CLOCK enviromental variable, however it should investigated if that is sufficent, as the YARP_CLOCK enviromental variable is readed only in the yarp init function : https://github.com/robotology/yarp/blob/0e862d8daec639d74d94f79460f7c21aff619072/src/libYARP_OS/src/Network.cpp#L620 .

[traversaro]

cc @gabrielenava @S-Dafarra

[yeshasvitirupachuri]

@diegoferigo FYI

[francesco-romano]

#46 should fix this issue

[diegoferigo]

@DanielePucci The ImpedanceControl didn't have YARP_CLOCK set. Exporting this variable will affect the filtering of the speed signal as we talked about f2f, and it is an alternative solution of setting the readSpeedAccFromControlBoard variable in yarpWholeBodyInterface.ini.

See also robotology-legacy/WBI-Toolbox-controllers#4

[DanielePucci]

Mmmm it seems weird to me since I'm pretty sure that torqueBalancing back in time did have the synchroniser correctly set.

[diegoferigo]

The synchronizer is a different thing as far as I understood. Despite processes are spawned by simulink, which controls gazebo stepping through the syncronizer you mentioned, they run by default using the system clock (i.e. the yarp default clock). In order to sync them with simulink, using YARP_CLOCK is required. It is a complicated loop, but in this way YARP_CLOCK is taken from gazebo, which is set step after step as said by simulink.

[traversaro]

I think this was solved in #56 .

[diegoferigo]

PR #56 removed the abstraction layer provided by the whole-body-interface. In its yarp implementation we were filtering the estimated quantities with tunable low-pass filters.

Right now the measurements provided by the GetMeasurement block are the raw signals, and it is user's responsibility to filter them properly with the DiscreteFilter introduced in #47.

This does not mean that the YARP_CLOCK env variable should not be set, we still need it in order to set the network clock as the default clock. I remember that during the last year the clock functionality in YARP has been rewritten, I'm not sure if I am missing something. @traversaro

[traversaro]

This does not mean that the YARP_CLOCK env variable should not be set, we still need it in order to set the network clock as the default clock. I remember that during the last year the clock functionality in YARP has been rewritten, I'm not sure if I am missing something. @traversaro

You are right: I did not think that while we removed the estimator threads, there are several threads that are still running and need to be synchronized with the simulated network clock, such as the threads created in the client devices such as the remote_controlboard. To avoid fake timeouts, those threads need to be synchronized with the simulation, and so you need the YARP RateThread's in the process to obey to the network clock, via YARP_CLOCK or any equivalent mean. This is another problem that we would not have when running Gazebo (or any other simulator) directly in the simulation master process, as described in #164 .

At this point to be honest, I am not sure what this issue is about. The original problem statement was

The Gazebo simulation and the estimation threads in charge of estimating joint position/velocities/etc are not synchronised even when using the appropriate block, and this introduces an undesired machine-dependence of the controllers performances.

I think this is the problem related to using any non-synchronized pub/sub or similar framework to communicate with the simulator, that can be solved only running properly synchronized simulations or co-simulations. Is this issue a duplicate of #164 ? : D

[diegoferigo]

I think this issue can be closed since the solution is to use the YARP_CLOCK env variable. In order to avoid it, we should go towards #164.