Run two parallel simulations of a gz-sim-yarp-plugins-enabled robot #153

xela-95 · 2024-04-08T11:54:32Z

Closes #147

xela-95 · 2024-04-08T12:00:26Z

In b460db8, two simualtions of the same world are instantiated and launched together, using TestFixture class provided by gz-sim.

Notes:

In this preliminary attempt the model does not contain any gz-sim-yarp-plugin;
Running the server with the blocking parameter set to false allow to run each simulation in a separate thread.
I'm having difficulties to extend the method using a vector of TestFixtures, because when I try to call the emplace_back passing the world path, I get a compilation errors. I'm trying to debug this.

xela-95 · 2024-04-09T14:04:12Z

With bb537e5 the unit test ControlBoardOnMultipleGazeboInstances uses a coupled pendulum model with a controlboard plugin.

Now the unit test fails, since there is an error when trying to set two devices in DeviceRegister with the same key.

It is then time to make a modification to the way the keys are generated.

@traversaro if I recall well you suggested prefixing the current device key with the memory address stored into the pointer in the call to:

DeviceRegistry::setDevice(std::string deviceDatabaseKey, yarp::dev::PolyDriver* device2add)

Moreover, I think that all the plugin singletons key should be updated in the same way, since for example, we should be able to store the data for the same controlboard device belonging to the same model used in two parallel simulations.

Do you foresee any troubles in progressing on this path?

…oncurrently In this first version no gz-sim-yarp-plugin is used

- introduced controlboard plugin - check that DeviceRegistry has the correct number of device keys --> FAILING

…set on DeviceRegistry Plugins modified: - basestate - camera - controlboard - forcetorque - imu - laser

This is done by: - removing the data singleton - adding a new header file containing the data structure and a custom interface to be implemented by force torque driver

Fix also `ControlBoardCommonsTest` to use `DeviceRegistry` in place of the singleton and remove expectations on device ids (since they are an internal implementation detail).

xela-95 · 2024-04-15T10:18:52Z

In the last 5 commits I've restructured the data management mechanisms of the plugins that previously made use of the singletons (see #147 (comment)). Now the only singleton in the codebase is constituted by the DeviceRegistry class.

…en different simulations. - Update model used in unit test to a double pendulum (the two simulations outcomes diverge quickly) - Update unit test

xela-95 · 2024-04-16T15:34:44Z

With c345e4c I updated the unit test to use a double pendulum, such that the reading of the second joint would present numerical differences pretty quickly.

Here's a video of the simulation of such model:

2024-04-16_09-49-00.mp4

Now to make the unit test pass I used the uglier way possible, but also the only one that seems to work at the moment, i.e. using the ECM memory address in the device id (along the scoped name of the device and the yarpDeviceName property).

I'm restructuring the code and the unit test in order to make them pass with the new modifications introduced, thing that is taking a bit of time.

- Update DeviceRegistry::getDevicesAsPolyDriverList method - Add pragma once direcgtive to DeviceRegistry - Remove check to BaseStateDriver raising exeption with new data management - Move initialization of Camera buffers after Driver has been opened - Add test-helpers library and TestHelpers class - Fix Imu test - Update ControlBoardPositionDirectTest (still failing)

xela-95 · 2024-04-17T08:22:57Z

There are still three tests failing:

ControlBoardTorqueControlTest (SEGFAULT)
ControlBoardPositionDirectControlTest (Subprocess aborted)
ControlBoardPositionControlTest (SEGFAULT)

…retrieval

xela-95 · 2024-04-17T10:20:13Z

What was causing the strange errors (really difficult to debug) the unit test https://github.com/robotology/gz-sim-yarp-plugins/actions/runs/8718567060/job/23916088302#step:12:217 was the use of the TestHelpers method

gz-sim-yarp-plugins/tests/test-helpers/TestHelpers.hh

Lines 14 to 32 in 9dc1044

    
           template <typename T> std::vector<T*> static getDevicesOfType() 
        
           { 
        
               auto deviceIds = DeviceRegistry::getHandler()->getDevicesKeys(); 
        
               std::vector<T*> devices{}; 
        
               // Get the controlboard devices 
        
               for (auto& deviceId : deviceIds) 
        
               { 
        
                   auto polyDriver = DeviceRegistry::getHandler()->getDevice(deviceId); 
        
                   T* driver = nullptr; 
        
                   auto viewOk = polyDriver->view(driver); 
        
                   if (viewOk && driver) 
        
                   { 
        
                       devices.push_back(driver); 
        
                   } 
        
               } 
        
               std::cerr << "Total devices registered: " << deviceIds.size() 
        
                         << ", of which of selected type: " << devices.size() << std::endl; 
        
               return devices; 
        
           }

I created this to help retrieve only a vector of devices already casted to the interface needed in the test. For example, if the model under test has different gz-sim-yarp-plugins defined, I want to perform tests only on ControlBoardDriver devices, but the way in which I implemented it does not work and make the tests crash in unexpected ways.

Before it was easy to reconstruct manually the deviceId since it was only the composition of the device scoped name and the yarpDeviceName (even if this is a code smell since it was relying on the internal implementation of the key string). So maybe now we should find an easy way to query the DeviceRegistry filtering the device to return based on some properties it has (type of Device, model to which it belongs, etc).

In order to be sure that the two simulations are actually distinct the joint is controlled in position and different references are given to the two simulations.

xela-95 · 2024-04-17T13:53:16Z

Since leaving a pendulum freely swing was not giving clear evidence of having two different simulations running concurrently, to assess this in a clear way I updated the test as suggested by @S-Dafarra: now two simulations of a single pendulum (from the same model) in which the joint is position controlled, receive two different reference positions. in this way we can check if the two joints are able to reach their setpoint.

Luckily this was the case! So I can finally conclude that we are able to simulate concurrently the same model on gazebo! 🥳

xela-95 · 2024-04-17T13:55:23Z

This is the outcome of the unit test in which in one simulation the reference position is 10 degrees, and in the other is 5 degrees:

Start 10: ControlBoardOnMultipleGazeboInstancesTest

10: Test command: /home/acroci/repos/gz-sim-yarp-plugins/build/bin/ControlBoardOnMultipleGazeboInstancesTest
10: Working Directory: /home/acroci/repos/gz-sim-yarp-plugins/build/tests/controlboard
10: Environment variables: 
10:  GZ_SIM_SYSTEM_PLUGIN_PATH=/home/acroci/repos/gz-sim-yarp-plugins/build/lib
10: Test timeout computed to be: 1500
10: Running main() from /home/acroci/repos/gz-sim-yarp-plugins/build/_deps/googletest-src/googletest/src/gtest_main.cc
10: [==========] Running 1 test from 1 test suite.
10: [----------] Global test environment set-up.
10: [----------] 1 test from ControlBoardOnMultipleGazeboInstances
10: [ RUN      ] ControlBoardOnMultipleGazeboInstances.StartConcurrentGazeboInstances
10: [WARNING] File specified with a relative path:  ../../../tests/controlboard/conf/gazebo_controlboard_multiple_instances.ini , resolved to:  /home/acroci/repos/gz-sim-yarp-plugins/build/tests/controlboard/../../../tests/controlboard/conf/gazebo_controlboard_multiple_instances.ini . It is recommended to use a URI.
10: [INFO] Yarp configuration file loaded:  /home/acroci/repos/gz-sim-yarp-plugins/build/tests/controlboard/../../../tests/controlboard/conf/gazebo_controlboard_multiple_instances.ini
10: [INFO] gz-sim-yarp-controlboard-system: configuration of device  controlboard_plugin_device2  loaded
10: [DEBUG] gz-sim-yarp-controlboard-system : robot scoped name:  model/triple_pendulum
10: ==================== ecm address for generated id: 0x58490cc5e5e8
10: [DEBUG] gz-sim-yarp-controlboard-system : deviceId:  0x58490cc5e5e8/model/triple_pendulum/controlboard_plugin_device2
10: [DEBUG] gz-sim-yarp-controlboard-system: initialConfiguration:  0.0
10: [DEBUG] |yarp.dev.PolyDriver|gazebo_controlboard| Parameters are (LIMITS (jntPosMax (10000.0)) (jntPosMin (-10000.0))) (POSITION_CONTROL (controlUnits metric_units) (controlLaw joint_pid_gazebo_v1) (kp (3000.0)) (kd (2.0)) (ki (0.100000000000000005551)) (maxInt (99999)) (maxOutput (99999)) (shift (0.0)) (ko (0.0)) (stictionUp (0.0)) (stictionDwn (0.0))) (VELOCITY_CONTROL (controlUnits metric_units) (controlLaw joint_pid_gazebo_v1) (velocityControlImplementationType integrator_and_position_pid) (kp (500.0)) (kd (2.0)) (ki (0.100000000000000005551)) (maxInt (9999)) (maxOutput (9999)) (shift (0.0)) (ko (0.0)) (stictionUp (0.0)) (stictionDwn (0.0))) (controlBoardId "0x58490cc5e5e8/model/triple_pendulum/controlboard_plugin_device2") (device gazebo_controlboard) (initialConfiguration "0.0") (jointNames (joint1)) (yarpDeviceName controlboard_plugin_device2)
10: [INFO] |yarp.dev.PolyDriver|gazebo_controlboard| Created device <gazebo_controlboard>. See C++ class ControlBoardDriver for documentation.
10: [INFO] Found 1 joints from the plugin configuration.
10: [INFO] Found 2 joints from the model description.
10: [INFO] Joint  joint1  added to the control board data.
10: [WARNING] Group TRAJECTORY_GENERATION not found in plugin configuration. Defaults to minimum jerk trajectory.
10: [WARNING] Group TRAJECTORY_GENERATION not found in plugin configuration. Defaults trajectory generation reference speed and accelerations will be applied
10: [DEBUG] Joint  joint1  trajectory generation reference speed:  10  [deg/s]
10: [DEBUG] Joint  joint1  trajectory generation reference acceleration:  10  [deg/s^2]
10: [INFO] Initial configuration found, initializing trajectory generator with it
10: [INFO] Registered YARP device with instance name: 0x58490cc5e5e8/model/triple_pendulum/controlboard_plugin_device2
10: [WARNING] File specified with a relative path:  ../../../tests/controlboard/conf/gazebo_controlboard_multiple_instances.ini , resolved to:  /home/acroci/repos/gz-sim-yarp-plugins/build/tests/controlboard/../../../tests/controlboard/conf/gazebo_controlboard_multiple_instances.ini . It is recommended to use a URI.
10: [INFO] Yarp configuration file loaded:  /home/acroci/repos/gz-sim-yarp-plugins/build/tests/controlboard/../../../tests/controlboard/conf/gazebo_controlboard_multiple_instances.ini
10: [INFO] gz-sim-yarp-controlboard-system: configuration of device  controlboard_plugin_device2  loaded
10: [DEBUG] gz-sim-yarp-controlboard-system : robot scoped name:  model/triple_pendulum
10: ==================== ecm address for generated id: 0x58490e649a88
10: [DEBUG] gz-sim-yarp-controlboard-system : deviceId:  0x58490e649a88/model/triple_pendulum/controlboard_plugin_device2
10: [DEBUG] gz-sim-yarp-controlboard-system: initialConfiguration:  0.0
10: [DEBUG] |yarp.dev.PolyDriver|gazebo_controlboard| Parameters are (LIMITS (jntPosMax (10000.0)) (jntPosMin (-10000.0))) (POSITION_CONTROL (controlUnits metric_units) (controlLaw joint_pid_gazebo_v1) (kp (3000.0)) (kd (2.0)) (ki (0.100000000000000005551)) (maxInt (99999)) (maxOutput (99999)) (shift (0.0)) (ko (0.0)) (stictionUp (0.0)) (stictionDwn (0.0))) (VELOCITY_CONTROL (controlUnits metric_units) (controlLaw joint_pid_gazebo_v1) (velocityControlImplementationType integrator_and_position_pid) (kp (500.0)) (kd (2.0)) (ki (0.100000000000000005551)) (maxInt (9999)) (maxOutput (9999)) (shift (0.0)) (ko (0.0)) (stictionUp (0.0)) (stictionDwn (0.0))) (controlBoardId "0x58490e649a88/model/triple_pendulum/controlboard_plugin_device2") (device gazebo_controlboard) (initialConfiguration "0.0") (jointNames (joint1)) (yarpDeviceName controlboard_plugin_device2)
10: [INFO] |yarp.dev.PolyDriver|gazebo_controlboard| Created device <gazebo_controlboard>. See C++ class ControlBoardDriver for documentation.
10: [INFO] Found 1 joints from the plugin configuration.
10: [INFO] Found 2 joints from the model description.
10: [INFO] Joint  joint1  added to the control board data.
10: [WARNING] Group TRAJECTORY_GENERATION not found in plugin configuration. Defaults to minimum jerk trajectory.
10: [WARNING] Group TRAJECTORY_GENERATION not found in plugin configuration. Defaults trajectory generation reference speed and accelerations will be applied
10: [DEBUG] Joint  joint1  trajectory generation reference speed:  10  [deg/s]
10: [DEBUG] Joint  joint1  trajectory generation reference acceleration:  10  [deg/s^2]
10: [INFO] Initial configuration found, initializing trajectory generator with it
10: [INFO] Registered YARP device with instance name: 0x58490e649a88/model/triple_pendulum/controlboard_plugin_device2
10: Waiting for Gazebo simulation to finish...
10: Waiting for Gazebo simulation to finish...
10: [Msg] Found no publishers on /stats, adding root stats topic
10: [Msg] Found no publishers on /stats, adding root stats topic
10: [Msg] Found no publishers on /clock, adding root clock topic
10: [Msg] Found no publishers on /clock, adding root clock topic
10: [Dbg] [SimulationRunner.cc:545] Creating PostUpdate worker threads: 4
10: Creating PostUpdate worker threads: [Dbg] [SimulationRunner.cc:545] Creating PostUpdate worker threads: 4
10: [Dbg] [SimulationRunner.cc:556] Creating postupdate worker thread (0)
10: Creating postupdate worker thread (0)
10: [Dbg] [SimulationRunner.cc:556] Creating postupdate worker thread (1)
10: Creating postupdate worker thread (1)
10: [Dbg] [SimulationRunner.cc:556] Creating postupdate worker thread (2)
10: [Dbg] [SimulationRunner.cc:556] Creating postupdate worker thread (2)
10: [Msg] Serving scene information on [/world/tutorial_controlboard/scene/info]
10: [Msg] Serving scene information on [/world/tutorial_controlboard/scene/info]
10: [Msg] Serving graph information on [/world/tutorial_controlboard/scene/graph]
10: [Msg] Serving graph information on [/world/tutorial_controlboard/scene/graph]
10: [Msg] Serving full state on [/world/tutorial_controlboard/state]
10: [Msg] Serving full state on [/world/tutorial_controlboard/state]
10: [Msg] Serving full state (async) on [/world/tutorial_controlboard/state_async]
10: [Msg] Serving full state (async) on [/world/tutorial_controlboard/state_async]
10: [Msg] Publishing scene information on [/world/tutorial_controlboard/scene/info]
10: [Msg] Publishing scene information on [/world/tutorial_controlboard/scene/info]
10: [Msg] Publishing entity deletions on [/world/tutorial_controlboard/scene/deletion]
10: [Msg] Publishing entity deletions on [/world/tutorial_controlboard/scene/deletion]
10: [Msg] Publishing state changes on [/world/tutorial_controlboard/state]
10: [Msg] Publishing state changes on [/world/tutorial_controlboard/state]
10: [Msg] Publishing pose messages on [/world/tutorial_controlboard/pose/info]
10: [Msg] Publishing pose messages on [/world/tutorial_controlboard/pose/info]
10: [Msg] Publishing dynamic pose messages on [/world/tutorial_controlboard/dynamic_pose/info]
10: [Msg] Publishing dynamic pose messages on [/world/tutorial_controlboard/dynamic_pose/info]
10: Waiting for Gazebo simulation to finish...
10: Waiting for Gazebo simulation to finish...
10: Waiting for Gazebo simulation to finish...
10: Waiting for Gazebo simulation to finish...
10: Waiting for Gazebo simulation to finish...
10: Waiting for Gazebo simulation to finish...
10: Waiting for Gazebo simulation to finish...
10: Waiting for Gazebo simulation to finish...
10: Waiting for Gazebo simulation to finish...
10: Waiting for Gazebo simulation to finish...
10: Motion done simulation 1 in 1000 iterations
10: Motion done simulation 2 in 500 iterations
10: Final joint position simulation 1: 9.9983920082
10: Final joint position simulation 2: 4.9983734197
10: [Dbg] [SimulationRunner.cc:572] [Dbg] [SimulationRunner.cc:572] Exiting postupdate worker thread (1)
10: Exiting postupdate worker thread ([Dbg] [SimulationRunner.cc:572] Exiting postupdate worker thread (2)Exiting postupdate worker thread (0
10: )[Dbg] [SimulationRunner.cc:572] Exiting postupdate worker thread (0[Dbg] [SimulationRunner.cc:572] 0))[Dbg] [SimulationRunner.cc:572] Exiting postupdate worker thread (2)
10: 
10: 1)
10: [       OK ] ControlBoardOnMultipleGazeboInstances.StartConcurrentGazeboInstances (12327 ms)
10: [----------] 1 test from ControlBoardOnMultipleGazeboInstances (12327 ms total)
10: 
10: [----------] Global test environment tear-down
10: [==========] 1 test from 1 test suite ran. (12327 ms total)
10: [  PASSED  ] 1 test.
1/1 Test #10: ControlBoardOnMultipleGazeboInstancesTest ...   Passed   12.39 sec

xela-95 · 2024-04-17T14:41:39Z

I want to iterate a bit on the DeviceRegistry data structure, I will put again this PR into a draft.

Also: - adapt all plugins and tests to use the new interface - ConfigurationParsingFromFileTest unit test has still an error in the controlboard assertions, it has been commented out for now

xela-95 · 2024-04-18T19:07:09Z

In cd0cd2f I have done substantial restrucuring of the DeviceRegistry interface and of its internal data structure. I adapted all the plugins to use the new interface and finally also the unit tests.

I'm getting a strange behavior on the following unit test, in which is loaded a model containing all plugins we have developed, each one with its yarpConfigurationFile. The strange thing is that if I enable the lines related to the check of the ControlBoard (now commented out), the entire tests is messed up, with Gazebo logging errors in which it cannot find the plugins libraries and the DeviceRegistry holding only the driver related to the controlboard.

gz-sim-yarp-plugins/tests/commons/ConfigurationParsingFromFileTest.cc

Lines 41 to 48 in cd0cd2f

    
           // Test controlboard --> not working 
        
           // std::cerr << "Testing ControlBoard configuration" << std::endl; 
        
           // std::vector<yarp::dev::gzyarp::ControlBoardDriver*> cbDrivers 
        
           //     = 
        
           //     gzyarp::testing::TestHelpers::getDevicesOfType<yarp::dev::gzyarp::ControlBoardDriver>( 
        
           //         _ecm); 
        
           // EXPECT_EQ(cbDrivers.size(), 1); 
        
           // EXPECT_TRUE(cbDrivers[0] != nullptr);

The error log I got on my machine is the following:

   Start 11: ConfigurationParsingFromFileTest

11: Test command: /home/acroci/repos/gz-sim-yarp-plugins/build/bin/ConfigurationParsingFromFileTest
11: Working Directory: /home/acroci/repos/gz-sim-yarp-plugins/build/tests/commons
11: Environment variables: 
11:  LIBGL_ALWAYS_SOFTWARE=1
11:  GZ_SIM_SYSTEM_PLUGIN_PATH=/home/acroci/repos/gz-sim-yarp-plugins/build/lib
11:  GZ_SIM_RESOURCE_PATH=:/home/acroci/repos/gz-sim-yarp-plugins/tests/commons/..
11: Test timeout computed to be: 1500
11: Running main() from /home/acroci/repos/gz-sim-yarp-plugins/build/_deps/googletest-src/googletest/src/gtest_main.cc
11: [==========] Running 1 test from 1 test suite.
11: [----------] Global test environment set-up.
11: [----------] 1 test from ConfigurationParsingTest
11: [ RUN      ] ConfigurationParsingTest.LoadPluginsWithYarpConfigurationFile
11: [Err] [SystemLoader.cc:107] Failed to load system plugin: (Reason: No plugins detected in library)
11: - Requested plugin name: [gz::sim::systems::Physics]
11: - Requested library name: [gz-sim-physics-system]
11: - Resolved library path: [/home/acroci/mambaforge/envs/walking/lib/gz-sim-8/plugins/libgz-sim-physics-system.so]
11: [Err] [SystemLoader.cc:107] Failed to load system plugin: (Reason: No plugins detected in library)
11: - Requested plugin name: [gz::sim::systems::UserCommands]
11: - Requested library name: [gz-sim-user-commands-system]
11: - Resolved library path: [/home/acroci/mambaforge/envs/walking/lib/gz-sim-8/plugins/libgz-sim-user-commands-system.so]
11: [Err] [SystemLoader.cc:107] Failed to load system plugin: (Reason: No plugins detected in library)
11: - Requested plugin name: [gz::sim::systems::Sensors]
11: - Requested library name: [gz-sim-sensors-system]
11: - Resolved library path: [/home/acroci/mambaforge/envs/walking/lib/gz-sim-8/plugins/libgz-sim-sensors-system.so]
11: [Err] [SystemLoader.cc:107] Failed to load system plugin: (Reason: No plugins detected in library)
11: - Requested plugin name: [gz::sim::systems::SceneBroadcaster]
11: - Requested library name: [gz-sim-scene-broadcaster-system]
11: - Resolved library path: [/home/acroci/mambaforge/envs/walking/lib/gz-sim-8/plugins/libgz-sim-scene-broadcaster-system.so]
11: [Err] [SystemLoader.cc:107] Failed to load system plugin: (Reason: No plugins detected in library)
11: - Requested plugin name: [gzyarp::Camera]
11: - Requested library name: [gz-sim-yarp-camera-system]
11: - Resolved library path: [/home/acroci/repos/gz-sim-yarp-plugins/build/lib/libgz-sim-yarp-camera-system.so]
11: [Err] [SystemLoader.cc:107] Failed to load system plugin: (Reason: No plugins detected in library)
11: - Requested plugin name: [gzyarp::Laser]
11: - Requested library name: [gz-sim-yarp-laser-system]
11: - Resolved library path: [/home/acroci/repos/gz-sim-yarp-plugins/build/lib/libgz-sim-yarp-laser-system.so]
11: [Err] [SystemLoader.cc:107] Failed to load system plugin: (Reason: No plugins detected in library)
11: - Requested plugin name: [gzyarp::Imu]
11: - Requested library name: [gz-sim-yarp-imu-system]
11: - Resolved library path: [/home/acroci/repos/gz-sim-yarp-plugins/build/lib/libgz-sim-yarp-imu-system.so]
11: [Err] [SystemLoader.cc:107] Failed to load system plugin: (Reason: No plugins detected in library)
11: - Requested plugin name: [gzyarp::BaseState]
11: - Requested library name: [gz-sim-yarp-basestate-system]
11: - Resolved library path: [/home/acroci/repos/gz-sim-yarp-plugins/build/lib/libgz-sim-yarp-basestate-system.so]
11: [Err] [SystemLoader.cc:107] Failed to load system plugin: (Reason: No plugins detected in library)
11: - Requested plugin name: [gzyarp::ForceTorque]
11: - Requested library name: [gz-sim-yarp-forcetorque-system]
11: - Resolved library path: [/home/acroci/repos/gz-sim-yarp-plugins/build/lib/libgz-sim-yarp-forcetorque-system.so]
11: [Msg] Loading SDF world file[/home/acroci/repos/gz-sim-yarp-plugins/tests/commons/model_configuration_file.sdf].
11: [Msg] Serving entity system service on [/entity/system/add]
11: [INFO] File is a URI:  model://commons/conf/controlboard.ini
11: [INFO] Yarp configuration file loaded:  /home/acroci/repos/gz-sim-yarp-plugins/tests/commons/../commons/conf/controlboard.ini
11: [INFO] gz-sim-yarp-controlboard-system: configuration of device  controlboard_plugin_device  loaded
11: [DEBUG] gz-sim-yarp-controlboard-system : robot scoped name:  model/model_with_plugins
11: [DEBUG] gz-sim-yarp-controlboard-system : yarpDeviceName:  controlboard_plugin_device
11: [DEBUG] |yarp.dev.PolyDriver|gazebo_controlboard| Parameters are (LIMITS (jntPosMax (200.0)) (jntPosMin (-200.0))) (POSITION_CONTROL (controlUnits metric_units) (controlLaw joint_pid_gazebo_v1) (kp (3000.0)) (kd (2.0)) (ki (0.100000000000000005551)) (maxInt (99999)) (maxOutput (99999)) (shift (0.0)) (ko (0.0)) (stictionUp (0.0)) (stictionDwn (0.0))) (VELOCITY_CONTROL (controlUnits metric_units) (controlLaw joint_pid_gazebo_v1) (velocityControlImplementationType integrator_and_position_pid) (kp (500.0)) (kd (2.0)) (ki (0.100000000000000005551)) (maxInt (9999)) (maxOutput (9999)) (shift (0.0)) (ko (0.0)) (stictionUp (0.0)) (stictionDwn (0.0))) (controlBoardId "model/model_with_plugins") (device gazebo_controlboard) (jointNames (joint_12)) (yarpDeviceName controlboard_plugin_device)
11: [INFO] |yarp.dev.PolyDriver|gazebo_controlboard| Created device <gazebo_controlboard>. See C++ class ControlBoardDriver for documentation.
11: ======== setDevice called for controlboard_plugin_device
11: ========== controlboard driver set in device registry
11: [INFO] Found 1 joints from the plugin configuration.
11: [INFO] Found 1 joints from the model description.
11: [INFO] Joint  joint_12  added to the control board data.
11: [WARNING] Group TRAJECTORY_GENERATION not found in plugin configuration. Defaults to minimum jerk trajectory.
11: [WARNING] Group TRAJECTORY_GENERATION not found in plugin configuration. Defaults trajectory generation reference speed and accelerations will be applied
11: [DEBUG] Joint  joint_12  trajectory generation reference speed:  10  [deg/s]
11: [DEBUG] Joint  joint_12  trajectory generation reference acceleration:  10  [deg/s^2]
11: [WARNING] No initial configuration found, initializing trajectory generator with current values
11: [INFO] Registered YARP device with instance name: model/model_with_plugins/controlboard_plugin_device
11: [Err] [SystemLoader.cc:107] Failed to load system plugin: (Reason: No plugins detected in library)
11: - Requested plugin name: [gz::sim::systems::Physics]
11: - Requested library name: [gz-sim-physics-system]
11: - Resolved library path: [/home/acroci/mambaforge/envs/walking/lib/gz-sim-8/plugins/libgz-sim-physics-system.so]
11: [Err] [SystemLoader.cc:107] Failed to load system plugin: (Reason: No plugins detected in library)
11: - Requested plugin name: [gz::sim::systems::UserCommands]
11: - Requested library name: [gz-sim-user-commands-system]
11: - Resolved library path: [/home/acroci/mambaforge/envs/walking/lib/gz-sim-8/plugins/libgz-sim-user-commands-system.so]
11: [Err] [SystemLoader.cc:107] Failed to load system plugin: (Reason: No plugins detected in library)
11: - Requested plugin name: [gz::sim::systems::SceneBroadcaster]
11: - Requested library name: [gz-sim-scene-broadcaster-system]
11: - Resolved library path: [/home/acroci/mambaforge/envs/walking/lib/gz-sim-8/plugins/libgz-sim-scene-broadcaster-system.so]
11: [Dbg] [SystemManager.cc:74] Loaded system [gzyarp::ControlBoard] for entity [8]
11: [Msg] Loaded level [3]
11: [Msg] No systems loaded from SDF, loading defaults
11: [Dbg] [ServerConfig.cc:1016] Loaded (3) plugins from file [/home/acroci/.gz/sim/8/server.config]
11: [Msg] Serving world controls on [/world/commons_test/control], [/world/commons_test/control/state] and [/world/commons_test/playback/control]
11: [Msg] Serving GUI information on [/world/commons_test/gui/info]
11: [Msg] World [commons_test] initialized with [1ms] physics profile.
11: [Msg] Serving world SDF generation service on [/world/commons_test/generate_world_sdf]
11: [Msg] Serving world names on [/gazebo/worlds]
11: [Msg] Resource path add service on [/gazebo/resource_paths/add].
11: [Msg] Resource path get service on [/gazebo/resource_paths/get].
11: [Msg] Resource path resolve service on [/gazebo/resource_paths/resolve].
11: [Msg] Resource paths published on [/gazebo/resource_paths].
11: [Msg] Server control service on [/server_control].
11: Testing ControlBoard configuration
11: ======== getDevice called for model/model_with_plugins/controlboard_plugin_device
11: Total devices registered: 1, of which of selected type: 1
11: Testing Camera configuration
11: ======== getDevice called for model/model_with_plugins/controlboard_plugin_device
11: Total devices registered: 1, of which of selected type: 0
11: /home/acroci/repos/gz-sim-yarp-plugins/tests/commons/ConfigurationParsingFromFileTest.cc:78: Failure
11: Expected equality of these values:
11:   cameraDrivers.size()
11:     Which is: 0
11:   1
1/1 Test #11: ConfigurationParsingFromFileTest ...***Exception: SegFault  0.45 sec

xela-95 · 2024-04-19T10:00:31Z

I was able to resolve the error I was getting in #153 (comment), by not including the Driver classes (also, some of them have only .cpp files and no headers) but by calling the view() method on the PolyDriver asking for the interfaces implemented by each driver. In this case no error occurred.

I'm not sure of the reason behind this strange behavior but it could be due to the include of Drivers cpp files that were messing up some library (like the DeviceRegistry singleton).

traversaro · 2024-04-19T10:06:20Z

The problem was probably some kind of consequences of us breaking the one definition rule (that is the reason why you always include .h header files with declarations or inline definitions and never .cpp with non-inline definitions). Anyhow, if this now works that is great, is this ready for review?

traversaro

Minor comments.

plugins/controlboard/src/ControlBoard.cpp

plugins/controlboard/include/ControlBoard.hh

xela-95 · 2024-04-19T10:16:49Z

The problem was probably some kind of consequences of us breaking the one definition rule (that is the reason why you always include .h header files with declarations or inline definitions and never .cpp with non-inline definitions). Anyhow, if this now works that is great, is this ready for review?

Yep, I agree with you, I had an unpleasant meeting with "undefined behavior" in C++ 🤣

Yep it's ready for review!

traversaro · 2024-04-19T10:18:41Z

I guess at some point we need to understand how to get the ecm pointer given the server, but for sure this is out of scope for this PR.

xela-95 · 2024-04-19T10:19:33Z

I guess at some point we need to understand how to get the ecm pointer given the server, but for sure this is out of scope for this PR.

Yes, exactly; for now, this is the elephant in the room.

xela-95 · 2024-04-19T10:57:33Z

The unit test failing on the CI is quite scary:

ODE INTERNAL ERROR 1: assertion "!colliders_initialized" failed in dInitColliders() [collision_kernel.cpp:168]

gz-sim related issue: gazebosim/gz-sim#18

The unit test failing is launching two parallel Gazebo instances of different models (one sphere and one box bouncing on the ground). This error seems related to the ODE collision detector having issues due to some global variable. However it seems to happen only on the apt CI workflow on Ubuntu@Debug.

traversaro · 2024-04-19T11:06:55Z

Ah, that is an old foe. For parallel workflows we should either use another collision detection algorithm when using dart (dart supports several of them if I recall correctly), or using another physics engine. cc @diegoferigo as probably we experienced something similar when working on gym-ignition.

…and collision detector to avoid exception in concurrent simulations

xela-95 · 2024-04-19T14:02:19Z

Now the models used by concurrent tests use dart both as physics engine and collision detector. All the tests are passing.

Merging 🚀

traversaro · 2024-04-19T20:43:55Z

That is nice for the tests, but it also means that users will start getting the problem if they start using our APIs for parallel computation. We should at least document this once we will document the possibility of running multiserver. See also the discussion in gazebosim/gz-sim#18 (comment) .

xela-95 self-assigned this Apr 8, 2024

xela-95 force-pushed the feature/run-parallel-gz-instances branch 2 times, most recently from 5298c54 to 8886369 Compare April 9, 2024 13:10

xela-95 force-pushed the feature/run-parallel-gz-instances branch from 8886369 to bb537e5 Compare April 9, 2024 14:15

xela-95 added 7 commits April 12, 2024 16:47

Add unit test launching multiple gazebo instances of the same world c…

3312517

…oncurrently In this first version no gz-sim-yarp-plugin is used

Fix logs in DeviceRegistry.cc

c0914d2

Add getter method to DeviceRegistry to obtain a vector of device keys

f3e524b

Update ControlBoardOnMultipleGazeboInstances unit test

eeed3e7

- introduced controlboard plugin - check that DeviceRegistry has the correct number of device keys --> FAILING

Add DeviceIdGenerator class

0fa6268

Update plugins to use DeviceIdGenerator to generate the device id to …

47010f7

…set on DeviceRegistry Plugins modified: - basestate - camera - controlboard - forcetorque - imu - laser

Update data structure management of forcetorque plugin

a9e0afa

This is done by: - removing the data singleton - adding a new header file containing the data structure and a custom interface to be implemented by force torque driver

xela-95 force-pushed the feature/run-parallel-gz-instances branch from 78b794e to 3b7e63b Compare April 12, 2024 14:47

xela-95 added 4 commits April 12, 2024 17:35

Refactor data management of imu plugin

0a9a688

Refactor data management of laser plugin

2740f8e

Refactor data management of camera plugin

4173112

Refactor data management of basestate plugin

71589ef

xela-95 force-pushed the feature/run-parallel-gz-instances branch from 3b7e63b to 71589ef Compare April 12, 2024 15:36

Refactor data management of controlboard plugin

f0d2246

Fix also `ControlBoardCommonsTest` to use `DeviceRegistry` in place of the singleton and remove expectations on device ids (since they are an internal implementation detail).

- Update device Id generation using ECM pointer to discriminate betwe…

c345e4c

…en different simulations. - Update model used in unit test to a double pendulum (the two simulations outcomes diverge quickly) - Update unit test

xela-95 force-pushed the feature/run-parallel-gz-instances branch from 24b6e48 to a901e0f Compare April 17, 2024 08:16

xela-95 added 2 commits April 17, 2024 12:10

Fix ControlBoardTorqueControlTest

6c3b5f9

Fix ControlBoardPositionControlTest to use DeviceRegistry for device …

9dc1044

…retrieval

Add ConcurrentInstancesTest

ea4ca06

xela-95 added 2 commits April 17, 2024 14:45

Remove debug logs in DeviceIdGenerator.cpp

2ae346a

Update ControlBoardOnMultipleGazeboInstancesTest

4422925

In order to be sure that the two simulations are actually distinct the joint is controlled in position and different references are given to the two simulations.

xela-95 marked this pull request as ready for review April 17, 2024 13:55

xela-95 requested a review from traversaro April 17, 2024 13:56

xela-95 marked this pull request as draft April 17, 2024 14:41

Update DeviceRegistry interface and data structure

cd0cd2f

Also: - adapt all plugins and tests to use the new interface - ConfigurationParsingFromFileTest unit test has still an error in the controlboard assertions, it has been commented out for now

Fix ConfigurationParsingFromFile test

e00c04c

xela-95 marked this pull request as ready for review April 19, 2024 10:00

traversaro approved these changes Apr 19, 2024

View reviewed changes

plugins/controlboard/src/ControlBoard.cpp Outdated Show resolved Hide resolved

plugins/controlboard/src/ControlBoard.cpp Outdated Show resolved Hide resolved

plugins/controlboard/include/ControlBoard.hh Outdated Show resolved Hide resolved

xela-95 added 2 commits April 19, 2024 12:32

Minor improvements to ControlBoard

fd8043f

Minor updates to commons tests

d580721

xela-95 added 2 commits April 19, 2024 14:36

Update sdf models used in ConcurrentInstancesTest to use DART engine …

42ff8cf

…and collision detector to avoid exception in concurrent simulations

Update pendulum_multiple_gz_instances.sdf to use DART

b57c41c

xela-95 merged commit 870b1d9 into main Apr 19, 2024
7 checks passed

xela-95 deleted the feature/run-parallel-gz-instances branch April 19, 2024 14:02

traversaro mentioned this pull request Apr 19, 2024

Support for multiple worlds gazebosim/gz-sim#18

Open

traversaro mentioned this pull request Aug 21, 2024

ConcurrentInstancesTest and ControlBoardOnMultipleGazeboInstancesTest non-deterministic crash #201

Closed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Run two parallel simulations of a gz-sim-yarp-plugins-enabled robot #153

Run two parallel simulations of a gz-sim-yarp-plugins-enabled robot #153

xela-95 commented Apr 8, 2024

xela-95 commented Apr 8, 2024

xela-95 commented Apr 9, 2024 •

edited

Loading

xela-95 commented Apr 15, 2024

xela-95 commented Apr 16, 2024

xela-95 commented Apr 17, 2024

xela-95 commented Apr 17, 2024 •

edited

Loading

xela-95 commented Apr 17, 2024

xela-95 commented Apr 17, 2024

xela-95 commented Apr 17, 2024

xela-95 commented Apr 18, 2024 •

edited

Loading

xela-95 commented Apr 19, 2024

traversaro commented Apr 19, 2024

traversaro left a comment

xela-95 commented Apr 19, 2024

traversaro commented Apr 19, 2024

xela-95 commented Apr 19, 2024

xela-95 commented Apr 19, 2024

traversaro commented Apr 19, 2024

xela-95 commented Apr 19, 2024

traversaro commented Apr 19, 2024

Run two parallel simulations of a gz-sim-yarp-plugins-enabled robot #153

Run two parallel simulations of a gz-sim-yarp-plugins-enabled robot #153

Conversation

xela-95 commented Apr 8, 2024

xela-95 commented Apr 8, 2024

xela-95 commented Apr 9, 2024 • edited Loading

xela-95 commented Apr 15, 2024

xela-95 commented Apr 16, 2024

xela-95 commented Apr 17, 2024

xela-95 commented Apr 17, 2024 • edited Loading

xela-95 commented Apr 17, 2024

xela-95 commented Apr 17, 2024

xela-95 commented Apr 17, 2024

xela-95 commented Apr 18, 2024 • edited Loading

xela-95 commented Apr 19, 2024

traversaro commented Apr 19, 2024

traversaro left a comment

Choose a reason for hiding this comment

xela-95 commented Apr 19, 2024

traversaro commented Apr 19, 2024

xela-95 commented Apr 19, 2024

xela-95 commented Apr 19, 2024

traversaro commented Apr 19, 2024

xela-95 commented Apr 19, 2024

traversaro commented Apr 19, 2024

xela-95 commented Apr 9, 2024 •

edited

Loading

xela-95 commented Apr 17, 2024 •

edited

Loading

xela-95 commented Apr 18, 2024 •

edited

Loading