diff --git a/tests/common/utils.py b/tests/common/utils.py
index 8f7b152a0..40e7960e4 100644
--- a/tests/common/utils.py
+++ b/tests/common/utils.py
@@ -3,6 +3,7 @@
# GNU Lesser General Public License v2.1 or any later version.
import pytest
+import dataclasses
from typing import Tuple
import gym_ignition_models
from gym_ignition.utils import misc
@@ -188,3 +189,45 @@ def get_empty_world_sdf() -> str:
world_file = misc.string_to_file(world_sdf_string)
return world_file
+
+
+@dataclasses.dataclass
+class SphereURDF:
+
+ mass: float = 5.0
+ radius: float = 0.1
+ restitution: float = 0
+
+ def urdf(self) -> str:
+
+ i = 2.0 / 5 * self.mass * self.radius * self.radius
+ urdf = f"""
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ {self.restitution}
+
+
+
+ """
+
+ return urdf
diff --git a/tests/test_scenario/test_multi_world.py b/tests/test_scenario/test_multi_world.py
index 2f1c063dd..768bd69d9 100644
--- a/tests/test_scenario/test_multi_world.py
+++ b/tests/test_scenario/test_multi_world.py
@@ -5,19 +5,23 @@
import pytest
pytestmark = pytest.mark.scenario
+import numpy as np
from ..common import utils
-from scenario import gazebo as scenario
+import gym_ignition_models
+from gym_ignition.utils import misc
+from scenario import core as scenario_core
+from scenario import gazebo as scenario_gazebo
from ..common.utils import gazebo_fixture as gazebo
# Set the verbosity
-scenario.set_verbosity(scenario.Verbosity_debug)
+scenario_gazebo.set_verbosity(scenario_gazebo.Verbosity_debug)
@pytest.mark.parametrize("gazebo",
[(0.001, 1.0, 1)],
indirect=True,
ids=utils.id_gazebo_fn)
-def test_insert_multiple_worlds(gazebo: scenario.GazeboSimulator):
+def _test_insert_multiple_worlds(gazebo: scenario_gazebo.GazeboSimulator):
empty_world_sdf = utils.get_empty_world_sdf()
assert gazebo.insert_world_from_sdf(empty_world_sdf, "myWorld1")
@@ -41,7 +45,7 @@ def test_insert_multiple_worlds(gazebo: scenario.GazeboSimulator):
[(0.001, 1.0, 1)],
indirect=True,
ids=utils.id_gazebo_fn)
-def test_insert_multiple_world(gazebo: scenario.GazeboSimulator):
+def _test_insert_multiple_world(gazebo: scenario_gazebo.GazeboSimulator):
multi_world_sdf = utils.get_multi_world_sdf_file()
@@ -64,7 +68,7 @@ def test_insert_multiple_world(gazebo: scenario.GazeboSimulator):
[(0.001, 1.0, 1)],
indirect=True,
ids=utils.id_gazebo_fn)
-def test_insert_multiple_world_rename(gazebo: scenario.GazeboSimulator):
+def _test_insert_multiple_world_rename(gazebo: scenario_gazebo.GazeboSimulator):
multi_world_sdf = utils.get_multi_world_sdf_file()
@@ -88,7 +92,7 @@ def test_insert_multiple_world_rename(gazebo: scenario.GazeboSimulator):
[(0.001, 1.0, 1)],
indirect=True,
ids=utils.id_gazebo_fn)
-def test_insert_world_multiple_calls(gazebo: scenario.GazeboSimulator):
+def _test_insert_world_multiple_calls(gazebo: scenario_gazebo.GazeboSimulator):
single_world_sdf = utils.get_empty_world_sdf()
@@ -107,3 +111,99 @@ def test_insert_world_multiple_calls(gazebo: scenario.GazeboSimulator):
assert world2.name() == "world2"
assert world1.id() != world2.id()
+
+
+@pytest.mark.parametrize("gazebo, solver",
+ [((0.001, 2.0, 1), "pgs"),
+ ((0.001, 2.0, 1), "dantzig")],
+ indirect=["gazebo"],
+ ids=utils.id_gazebo_fn)
+def test_multi_world_simulation(gazebo: scenario_gazebo.GazeboSimulator,
+ solver: str):
+
+ # Empty DART world with bullet as collision detector.
+ # It should prevent ODE crashes in a multi-world setting due to its static variables.
+ world_sdf_string = f"""
+
+
+
+
+
+ bullet
+
+ {solver}
+
+
+
+
+ true
+ 0 0 10 0 0 0
+ 1 1 1 1
+ 0.5 0.5 0.5 1
+
+ 1000
+ 0.9
+ 0.01
+ 0.001
+
+ -0.5 0.1 -0.9
+
+
+
+ """
+
+ # Create a tmp file from the SDF string
+ world_sdf_file = misc.string_to_file(string=world_sdf_string)
+
+ # Load two different worlds
+ assert gazebo.insert_world_from_sdf(world_sdf_file, "dart1")
+ assert gazebo.insert_world_from_sdf(world_sdf_file, "dart2")
+
+ # Initialize the simulator
+ assert gazebo.initialize()
+
+ # gazebo.gui()
+ # import time
+ # time.sleep(1)
+ # gazebo.run(paused=True)
+
+ sphere_urdf_string = utils.SphereURDF(restitution=0.8).urdf()
+ sphere_urdf = misc.string_to_file(sphere_urdf_string)
+ ground_plane_urdf = gym_ignition_models.get_model_file(robot_name="ground_plane")
+
+ # Pose of the sphere
+ sphere_pose = scenario_core.Pose([0, 0, 0.5], [1, 0, 0, 0])
+
+ # Populate the first DART world
+ world1 = gazebo.get_world("dart1")
+ world1.set_physics_engine(scenario_gazebo.PhysicsEngine_dart)
+ world1.insert_model(ground_plane_urdf)
+ world1.insert_model(sphere_urdf, sphere_pose)
+ sphere1 = world1.get_model(model_name="sphere_model")
+
+ # Populate the second DART world
+ world2 = gazebo.get_world("dart2")
+ world2.set_physics_engine(scenario_gazebo.PhysicsEngine_dart)
+ world2.insert_model(ground_plane_urdf)
+ world2.insert_model(sphere_urdf, sphere_pose)
+ sphere2 = world2.get_model(model_name="sphere_model")
+
+ # Integration time
+ dt = gazebo.step_size() * gazebo.steps_per_run()
+
+ # Enable contacts
+ sphere1.enable_contacts(True)
+ sphere2.enable_contacts(True)
+
+ for _ in np.arange(start=0.0, stop=5.0, step=dt):
+
+ # Run the simulator
+ assert gazebo.run()
+
+ # Check that both worlds evolve similarly
+ assert sphere1.base_position() == \
+ pytest.approx(sphere2.base_position(), abs=0.001)
+ assert sphere1.base_world_linear_velocity() == \
+ pytest.approx(sphere2.base_world_linear_velocity(), abs=0.001)
+ assert sphere1.base_world_angular_velocity() == \
+ pytest.approx(sphere2.base_world_angular_velocity())