Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Test a multi-world simulation with DART and bullet collision detector #368

Merged
merged 2 commits into from
Jul 1, 2021
Merged

Test a multi-world simulation with DART and bullet collision detector #368

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
7880882
Add helper class to insert a Sphere
diegoferigo Jul 1, 2021
4b09710
Test a multi-world simulation with DART and bullet collision detector
diegoferigo Jul 1, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
43 changes: 43 additions & 0 deletions tests/common/utils.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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"""
<robot name="sphere_model" xmlns:xacro="http://www.ros.org/wiki/xacro">
<link name="sphere">
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="{self.mass}"/>
<inertia ixx="{i}" ixy="0" ixz="0" iyy="{i}" iyz="0" izz="{i}"/>
</inertial>
<visual>
<geometry>
<sphere radius="{self.radius}"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
</visual>
<collision>
<geometry>
<sphere radius="{self.radius}"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 0"/>
</collision>
</link>
<gazebo reference="sphere">
<collision><surface><bounce>
<restitution_coefficient>{self.restitution}</restitution_coefficient>
</bounce></surface></collision>
</gazebo>
</robot>
"""

return urdf
112 changes: 106 additions & 6 deletions tests/test_scenario/test_multi_world.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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")
Expand All @@ -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()

Expand All @@ -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()

Expand All @@ -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()

Expand All @@ -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"""
<?xml version="1.0" ?>
<sdf version="1.7">
<world name="default">
<physics default="true" type="dart">
<dart>
<collision_detector>bullet</collision_detector>
<solver>
<solver_type>{solver}</solver_type>
</solver>
</dart>
</physics>
<light type="directional" name="sun">
<cast_shadows>true</cast_shadows>
<pose>0 0 10 0 0 0</pose>
<diffuse>1 1 1 1</diffuse>
<specular>0.5 0.5 0.5 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
</light>
</world>
</sdf>
"""

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