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())