def test_world_api(gazebo: scenario.GazeboSimulator):
assert gazebo.initialize()
world = gazebo.get_world()
gravity = [0, 0, 10.0]
assert world.set_gravity(gravity)
assert world.gravity() == pytest.approx(gravity)
assert len(world.model_names()) == 0
# Insert a model from an non existent file
assert not world.insert_model("")
# Insert first cube with default name and pose
cube_urdf = utils.get_cube_urdf()
assert world.insert_model(cube_urdf)
assert len(world.model_names()) == 1
default_model_name = scenario.get_model_name_from_sdf(cube_urdf, 0)
assert default_model_name in world.model_names()
cube1 = world.get_model(default_model_name)
# TODO: assert cube1
assert cube1.name() == default_model_name
assert cube1.base_position() == pytest.approx([0, 0, 0])
assert cube1.base_orientation() == pytest.approx([1, 0, 0, 0])
# Inserting a model with the same name should fail
assert not world.insert_model(cube_urdf)
assert len(world.model_names()) == 1
# Insert second cube with custom name and pose
custom_model_name = "other_cube"
assert custom_model_name != default_model_name
custom_model_pose = core.Pose([1, 1, 0], [0, 0, 0, 1])
assert world.insert_model(cube_urdf, custom_model_pose, custom_model_name)
assert custom_model_name in world.model_names()
assert len(world.model_names()) == 2
cube2 = world.get_model(custom_model_name)
assert cube1 != cube2
# TODO: assert cube2
assert cube2.name() == custom_model_name
assert cube2.base_position() == pytest.approx(custom_model_pose.position)
assert cube2.base_orientation() == pytest.approx(custom_model_pose.orientation)
# Remove the first model (requires either a paused or unpaused step)
assert world.remove_model(default_model_name)
assert len(world.model_names()) == 2
gazebo.run(paused=True)
assert len(world.model_names()) == 1
# Without the physics system, the time should not increase
gazebo.run()
gazebo.run()
gazebo.run()
assert world.time() == 0.0
def test_initialization(gazebo: scenario.GazeboSimulator):
ok = gazebo.initialize()
rtf = gazebo.real_time_factor()
step_size = gazebo.step_size()
iterations = gazebo.steps_per_run()
if step_size <= 0:
assert not ok
assert not gazebo.initialized()
assert not gazebo.run()
if rtf <= 0:
assert not ok
assert not gazebo.initialized()
assert not gazebo.run()
if iterations <= 0:
assert not ok
assert not gazebo.initialized()
assert not gazebo.run()
if rtf > 0 and iterations > 0 and step_size > 0:
assert ok
assert gazebo.initialized()
def test_model_base_pose(gazebo: scenario.GazeboSimulator):
assert gazebo.initialize()
gym_ignition_model_name = "pendulum"
model = get_model(gazebo, gym_ignition_model_name)
assert gym_ignition_model_name in gazebo.get_world().model_names()
assert model.base_frame() == "support"
# assert model.set_base_frame("support") # TODO: Not yet supported
# assert model.set_base_frame("pendulum") # TODO: Not yet supported
# Check that the pose is the identical
gazebo.run(paused=True)
assert model.base_position() == pytest.approx([0, 0, 0])
assert model.base_orientation() == pytest.approx([1, 0, 0, 0])
# Reset the base pose
new_base_pose = dict(position=[5, 5, 0], orientation=[0, 1, 0, 0])
assert model.reset_base_pose(new_base_pose["position"],
new_base_pose["orientation"])
# Before stepping the simulation the pose should be the initial one
assert model.base_position() == pytest.approx([0, 0, 0])
assert model.base_orientation() == pytest.approx([1, 0, 0, 0])
# Step the simulator and check that the pose changes
gazebo.run(paused=True)
assert model.base_position() == pytest.approx(new_base_pose["position"])
assert model.base_orientation() == pytest.approx(
new_base_pose["orientation"])
def test_cube_contact(gazebo: scenario.GazeboSimulator,
get_model_str: Callable):
assert gazebo.initialize()
world = gazebo.get_world().to_gazebo()
# Insert the Physics system
assert world.set_physics_engine(scenario.PhysicsEngine_dart)
# Insert the ground plane
assert world.insert_model(gym_ignition_models.get_model_file("ground_plane"))
assert len(world.model_names()) == 1
# Insert the cube
cube_urdf = misc.string_to_file(get_model_str())
assert world.insert_model(cube_urdf,
core.Pose([0, 0, 0.15], [1., 0, 0, 0]),
"cube")
assert len(world.model_names()) == 2
# Get the cube
cube = world.get_model("cube")
# Enable contact detection
assert not cube.contacts_enabled()
assert cube.enable_contacts(enable=True)
assert cube.contacts_enabled()
# The cube was inserted floating in the air with a 5cm gap wrt to ground.
# There should be no contacts.
gazebo.run(paused=True)
assert not cube.get_link("cube").in_contact()
assert len(cube.contacts()) == 0
# Make the cube fall for 150ms
for _ in range(150):
gazebo.run()
# There should be only one contact, between ground and the cube
assert cube.get_link("cube").in_contact()
assert len(cube.contacts()) == 1
# Get the contact
contact_with_ground = cube.contacts()[0]
assert contact_with_ground.body_a == "cube::cube"
assert contact_with_ground.body_b == "ground_plane::link"
# Check contact points
for point in contact_with_ground.points:
assert point.normal == pytest.approx([0, 0, 1])
# Check that the contact force matches the weight of the cube
z_forces = [point.force[2] for point in contact_with_ground.points]
assert np.sum(z_forces) == pytest.approx(-5 * world.gravity()[2], abs=0.1)
# Forces of all contact points are combined by the following method
assert cube.get_link("cube").contact_wrench() == \
pytest.approx([0, 0, np.sum(z_forces), 0, 0, 0])
def test_load_default_world(gazebo: scenario.GazeboSimulator):
assert gazebo.initialize()
world = gazebo.get_world()
assert world.id() != 0
assert world.time() == 0.0
assert world.name() == "default"
def test_pause(gazebo: scenario.GazeboSimulator):
assert gazebo.initialize()
assert not gazebo.running()
assert gazebo.pause()
assert gazebo.initialize()
assert not gazebo.running()
assert gazebo.pause()
assert gazebo.run(paused=True)
assert not gazebo.running()
assert gazebo.pause()
assert gazebo.run(paused=False)
assert not gazebo.running()
assert gazebo.pause()
def test_fuel_world(gazebo: scenario.GazeboSimulator):
# (setup) load a world that includes a fuel model
worlds_folder = Path(__file__) / ".." / ".." / "assets" / "worlds"
world_file = worlds_folder / "fuel_support.sdf"
assert gazebo.insert_world_from_sdf(str(world_file.resolve()))
assert gazebo.initialize()
assert gazebo.run(paused=True)
# the actual test
assert "ground_plane" in gazebo.get_world().model_names()
def test_load_default_world_from_file(gazebo: scenario.GazeboSimulator):
empty_world_sdf = utils.get_empty_world_sdf()
assert gazebo.insert_world_from_sdf(empty_world_sdf)
assert gazebo.initialize()
world = gazebo.get_world()
assert world.id() != 0
assert world.time() == 0.0
assert world.name() == "default"
def test_load_default_world(gazebo: scenario.GazeboSimulator):
assert gazebo.initialize()
assert gazebo.world_names()
assert len(gazebo.world_names()) == 1
world1 = gazebo.get_world()
assert world1
assert world1.name() in gazebo.world_names()
world2 = gazebo.get_world(gazebo.world_names()[0])
assert world2
assert world1.id() == world2.id()
assert world1.name() == world2.name()
def test_paused_step(gazebo: scenario.GazeboSimulator):
assert gazebo.initialize()
world = gazebo.get_world().to_gazebo()
assert world.insert_model(
gym_ignition_models.get_model_file("ground_plane"))
assert "ground_plane" in world.model_names()
gazebo.run(paused=True)
world.remove_model("ground_plane")
assert "ground_plane" in world.model_names()
gazebo.run(paused=True)
assert "ground_plane" not in world.model_names()
assert world.time() == 0.0
def test_model_core_api(gazebo: scenario.GazeboSimulator):
assert gazebo.initialize()
gym_ignition_model_name = "cartpole"
model = get_model(gazebo, gym_ignition_model_name)
assert model.id() != 0
assert model.valid()
assert model.name() == gym_ignition_model_name
assert len(model.link_names()) == model.nr_of_links()
assert len(model.joint_names()) == model.nr_of_joints()
assert model.set_controller_period(0.42)
assert model.controller_period() == 0.42
def test_insert_multiple_world(gazebo: scenario.GazeboSimulator):
multi_world_sdf = utils.get_multi_world_sdf_file()
assert gazebo.insert_worlds_from_sdf(multi_world_sdf)
assert gazebo.initialize()
assert "world1" in gazebo.world_names()
assert "world2" in gazebo.world_names()
world1 = gazebo.get_world("world1")
world2 = gazebo.get_world("world2")
assert world1.name() == "world1"
assert world2.name() == "world2"
assert world1.id() != world2.id()
def test_rename_default_world(gazebo: scenario.GazeboSimulator):
empty_world_sdf = utils.get_empty_world_sdf()
assert gazebo.insert_world_from_sdf(empty_world_sdf, "myWorld")
assert gazebo.initialize()
world = gazebo.get_world()
assert world.id() != 0
assert world.time() == 0.0
assert world.name() == "myWorld"
world1 = gazebo.get_world("myWorld")
assert world1.id() == world.id()
assert world1.time() == 0.0
assert world1.name() == "myWorld"
def test_model_joints(gazebo: scenario.GazeboSimulator):
assert gazebo.initialize()
gym_ignition_model_name = "panda"
model = get_model(gazebo, gym_ignition_model_name)
q = model.joint_positions()
assert pytest.approx(q) == [0.0] * model.dofs()
dq = model.joint_velocities()
assert pytest.approx(dq) == [0.0] * model.dofs()
assert model.reset_joint_positions([0.05] * model.dofs())
assert model.joint_positions() == pytest.approx([0.0] * model.dofs())
gazebo.run(paused=True)
assert model.joint_positions() == pytest.approx([0.05] * model.dofs())
assert model.joint_velocities() == pytest.approx([0.0] * model.dofs())
gazebo.run(paused=False)
assert model.joint_velocities() != pytest.approx([0.0] * model.dofs())
assert model.reset_joint_velocities([-0.1] * model.dofs())
assert model.joint_velocities() != pytest.approx([-0.1] * model.dofs())
gazebo.run(paused=True)
assert model.joint_velocities() == pytest.approx([-0.1] * model.dofs())
assert model.reset_joint_positions([0.0] * model.dofs())
assert model.reset_joint_velocities([0.0] * model.dofs())
joint_subset = model.joint_names()[0:4]
assert model.reset_joint_positions([-0.4] * len(joint_subset),
joint_subset)
assert model.reset_joint_velocities([3.0] * len(joint_subset),
joint_subset)
gazebo.run(paused=True)
assert model.joint_positions(joint_subset) == pytest.approx(
[-0.4] * len(joint_subset))
assert model.joint_velocities(joint_subset) == pytest.approx(
[3.0] * len(joint_subset))
assert model.joint_positions() == pytest.approx(
[-0.4] * len(joint_subset) + [0.0] *
(model.dofs() - len(joint_subset)))
assert model.joint_velocities() == pytest.approx(
[3.0] * len(joint_subset) + [0.0] * (model.dofs() - len(joint_subset)))
def test_insert_multiple_worlds(gazebo: scenario.GazeboSimulator):
empty_world_sdf = utils.get_empty_world_sdf()
assert gazebo.insert_world_from_sdf(empty_world_sdf, "myWorld1")
assert not gazebo.insert_world_from_sdf(empty_world_sdf, "myWorld1")
assert gazebo.insert_world_from_sdf(empty_world_sdf, "myWorld2")
assert gazebo.initialize()
assert "myWorld1" in gazebo.world_names()
assert "myWorld2" in gazebo.world_names()
my_world1 = gazebo.get_world("myWorld1")
my_world2 = gazebo.get_world("myWorld2")
assert my_world1.id() != my_world2.id()
assert my_world1.name() == "myWorld1"
assert my_world2.name() == "myWorld2"
def test_insert_multiple_world_rename(gazebo: scenario.GazeboSimulator):
multi_world_sdf = utils.get_multi_world_sdf_file()
assert not gazebo.insert_worlds_from_sdf(multi_world_sdf, ["only_one_name"])
assert gazebo.insert_worlds_from_sdf(multi_world_sdf, ["myWorld1", "myWorld2"])
assert gazebo.initialize()
assert "myWorld1" in gazebo.world_names()
assert "myWorld2" in gazebo.world_names()
world1 = gazebo.get_world("myWorld1")
world2 = gazebo.get_world("myWorld2")
assert world1.name() == "myWorld1"
assert world2.name() == "myWorld2"
assert world1.id() != world2.id()
def test_insert_world_multiple_calls(gazebo: scenario.GazeboSimulator):
single_world_sdf = utils.get_empty_world_sdf()
assert gazebo.insert_world_from_sdf(single_world_sdf)
assert not gazebo.insert_world_from_sdf(single_world_sdf)
assert gazebo.insert_world_from_sdf(single_world_sdf, "world2")
assert gazebo.initialize()
assert "default" in gazebo.world_names()
assert "world2" in gazebo.world_names()
world1 = gazebo.get_world("default")
world2 = gazebo.get_world("world2")
assert world1.name() == "default"
assert world2.name() == "world2"
assert world1.id() != world2.id()
def test_model_references(gazebo: scenario.GazeboSimulator):
assert gazebo.initialize()
gym_ignition_model_name = "cartpole"
model = get_model(gazebo, gym_ignition_model_name)
assert gym_ignition_model_name in gazebo.get_world().model_names()
assert model.set_joint_control_mode(core.JointControlMode_force)
assert model.set_joint_position_targets([0.5, -3])
assert model.joint_position_targets() == pytest.approx([0.5, -3])
assert model.joint_position_targets(["pivot"]) == pytest.approx([-3])
assert model.set_joint_velocity_targets([-0.1, 6])
assert model.joint_velocity_targets() == pytest.approx([-0.1, 6])
assert model.joint_velocity_targets(["pivot"]) == pytest.approx([6])
assert model.set_joint_acceleration_targets([-0, 3.14])
assert model.joint_acceleration_targets() == pytest.approx([-0, 3.14])
assert model.joint_acceleration_targets(["pivot"]) == pytest.approx([3.14])
assert model.set_joint_generalized_force_targets([20.1, -13])
assert model.joint_generalized_force_targets() == pytest.approx(
[20.1, -13])
assert model.joint_generalized_force_targets(["pivot"
]) == pytest.approx([-13])
assert model.set_base_pose_target((0, 0, 5), (0, 0, 0, 1.0))
assert model.set_base_orientation_target((0, 0, 1.0, 0))
assert model.base_position_target() == pytest.approx([0, 0, 5])
assert model.base_orientation_target() == pytest.approx([0, 0, 1.0, 0])
assert model.set_base_world_linear_velocity_target((1, 2, 3))
assert model.set_base_world_angular_velocity_target((4, 5, 6))
assert model.set_base_world_angular_acceleration_target((-1, -2, -3))
assert model.base_world_linear_velocity_target() == pytest.approx(
[1, 2, 3])
assert model.base_world_angular_velocity_target() == pytest.approx(
[4, 5, 6])
assert model.base_world_angular_acceleration_target() == pytest.approx(
[-1, -2, -3])
def test_world_physics_plugin(gazebo: scenario.GazeboSimulator):
assert gazebo.initialize()
world = gazebo.get_world()
dt = gazebo.step_size()
assert world.time() == 0
# Insert a cube
cube_urdf = utils.get_cube_urdf()
cube_name = "my_cube"
cube_pose = core.Pose([0, 0, 1.0], [1, 0, 0, 0])
assert world.insert_model(cube_urdf, cube_pose, cube_name)
assert cube_name in world.model_names()
cube = world.get_model(cube_name)
# There's no physics, the cube should not move
for _ in range(10):
gazebo.run()
assert cube.base_position() == cube_pose.position
assert world.time() == 0
# Insert the Physics system
assert world.set_physics_engine(scenario.PhysicsEngine_dart)
# After the first step, the physics catches up with time
gazebo.run()
assert world.time() == pytest.approx(11 * dt)
# The cube should start falling
assert cube.base_position()[2] < cube_pose.position[2]
gazebo.run()
assert world.time() == pytest.approx(12 * dt)
# Paused steps do not increase the time
gazebo.run(paused=True)
assert world.time() == pytest.approx(12 * dt)
def test_sim_time_starts_from_zero(gazebo: scenario.GazeboSimulator):
assert gazebo.initialize()
world = gazebo.get_world()
dt = gazebo.step_size()
assert world.time() == 0
assert world.set_physics_engine(scenario.PhysicsEngine_dart)
assert world.time() == 0
gazebo.run(paused=True)
assert world.time() == 0
gazebo.run(paused=False)
assert world.time() == dt
gazebo.run(paused=False)
assert world.time() == 2 * dt
gazebo.run(paused=False)
assert world.time() == pytest.approx(3 * dt)
def test_download_model_from_fuel(gazebo: scenario.GazeboSimulator):
assert gazebo.initialize()
# Get the default world
world = gazebo.get_world()
# Download a model from Fuel (testing a name with spaces)
model_name = "Electrical Box"
model_sdf = scenario.get_model_file_from_fuel(
f"https://fuel.ignitionrobotics.org/openrobotics/models/{model_name}", False
)
assert model_sdf
assert world.insert_model(model_sdf, core.Pose_identity())
assert model_name in world.model_names()
# Insert another model changing its name
other_model_name = "my_box"
other_model_pose = core.Pose([3.0, 0.0, 0.0], [1.0, 0, 0, 0])
assert world.insert_model(model_sdf, other_model_pose, other_model_name)
assert other_model_name in world.model_names()
assert gazebo.run()
def test_computed_torque_fixed_base(gazebo: scenario.GazeboSimulator):
assert gazebo.initialize()
step_size = gazebo.step_size()
# Get the default world
world = gazebo.get_world()
# Insert the physics
assert world.set_physics_engine(scenario.PhysicsEngine_dart)
# Get the panda urdf
panda_urdf = gym_ignition_models.get_model_file("panda")
# Insert the panda arm
model_name = "panda"
assert world.insert_model(panda_urdf, core.Pose_identity(), model_name)
# import time
# gazebo.gui()
# time.sleep(3)
# gazebo.run(paused=True)
# Get the model
panda = world.get_model(model_name).to_gazebo()
# Set the controller period
panda.set_controller_period(step_size)
# Insert the controller
assert panda.insert_model_plugin(*controllers.ComputedTorqueFixedBase(
kp=[10.0] * panda.dofs(),
ki=[0.0] * panda.dofs(),
kd=[3.0] * panda.dofs(),
urdf=panda_urdf,
joints=panda.joint_names(),
).args())
# Set the references
assert panda.set_joint_position_targets([0.0] * panda.dofs())
assert panda.set_joint_velocity_targets([0.0] * panda.dofs())
assert panda.set_joint_acceleration_targets([0.0] * panda.dofs())
joints_no_fingers = [
j for j in panda.joint_names() if j.startswith("panda_joint")
]
nr_of_joints = len(joints_no_fingers)
assert nr_of_joints > 0
# Reset the joints state
q0 = [np.deg2rad(45)] * nr_of_joints
dq0 = [0.1] * nr_of_joints
assert panda.reset_joint_positions(q0, joints_no_fingers)
assert panda.reset_joint_velocities(dq0, joints_no_fingers)
assert gazebo.run(True)
assert panda.joint_positions(joints_no_fingers) == pytest.approx(q0)
assert panda.joint_velocities(joints_no_fingers) == pytest.approx(dq0)
# Step the simulator for a couple of seconds
for _ in range(3000):
gazebo.run()
# Check that the the references have been reached
assert panda.joint_positions() == pytest.approx(
panda.joint_position_targets(), abs=np.deg2rad(1))
assert panda.joint_velocities() == pytest.approx(
panda.joint_velocity_targets(), abs=0.05)
# Apply an external force
assert (panda.get_link("panda_link4").to_gazebo().apply_world_force(
[100.0, 0, 0], 0.5))
for _ in range(4000):
assert gazebo.run()
# Check that the the references have been reached
assert panda.joint_positions() == pytest.approx(
panda.joint_position_targets(), abs=np.deg2rad(1))
assert panda.joint_velocities() == pytest.approx(
panda.joint_velocity_targets(), abs=0.05)
def test_run(gazebo: scenario.GazeboSimulator):
assert gazebo.initialize()
assert gazebo.run(paused=True)
assert gazebo.run()
def test_cube_multiple_contacts(gazebo: scenario.GazeboSimulator,
get_model_str: Callable):
assert gazebo.initialize()
world = gazebo.get_world().to_gazebo()
# Insert the Physics system
assert world.set_physics_engine(scenario.PhysicsEngine_dart)
# Insert the ground plane
assert world.insert_model(gym_ignition_models.get_model_file("ground_plane"))
assert len(world.model_names()) == 1
# Insert two cubes side to side with a 10cm gap
cube_urdf = misc.string_to_file(get_model_str())
assert world.insert_model(cube_urdf,
core.Pose([0, -0.15, 0.101], [1., 0, 0, 0]),
"cube1")
assert world.insert_model(cube_urdf,
core.Pose([0, 0.15, 0.101], [1., 0, 0, 0]),
"cube2")
assert len(world.model_names()) == 3
# Get the cubes
cube1 = world.get_model("cube1")
cube2 = world.get_model("cube2")
# Enable contact detection
assert not cube1.contacts_enabled()
assert cube1.enable_contacts(enable=True)
assert cube1.contacts_enabled()
# Enable contact detection
assert not cube2.contacts_enabled()
assert cube2.enable_contacts(enable=True)
assert cube2.contacts_enabled()
# The cubes were inserted floating in the air with a 1mm gap wrt to ground.
# There should be no contacts.
gazebo.run(paused=True)
assert not cube1.get_link("cube").in_contact()
assert not cube2.get_link("cube").in_contact()
assert len(cube1.contacts()) == 0
assert len(cube2.contacts()) == 0
# Make the cubes fall for 50ms
for _ in range(50):
gazebo.run()
assert cube1.get_link("cube").in_contact()
assert cube2.get_link("cube").in_contact()
assert len(cube1.contacts()) == 1
assert len(cube2.contacts()) == 1
# Now we make another cube fall above the gap. It will touch both cubes.
assert world.insert_model(cube_urdf,
core.Pose([0, 0, 0.301], [1., 0, 0, 0]),
"cube3")
assert len(world.model_names()) == 4
cube3 = world.get_model("cube3")
assert not cube3.contacts_enabled()
assert cube3.enable_contacts(enable=True)
assert cube3.contacts_enabled()
gazebo.run(paused=True)
assert not cube3.get_link("cube").in_contact()
assert len(cube3.contacts()) == 0
# Make the cube fall for 50ms
for _ in range(50):
gazebo.run()
# There will be two contacts, respectively with cube1 and cube2.
# Contacts are related to the link, not the collision elements. In the case of two
# collisions elements associated to the same link, contacts are merged together.
assert cube3.get_link("cube").in_contact()
assert len(cube3.contacts()) == 2
contact1 = cube3.contacts()[0]
contact2 = cube3.contacts()[1]
assert contact1.body_a == "cube3::cube"
assert contact2.body_a == "cube3::cube"
assert contact1.body_b == "cube1::cube"
assert contact2.body_b == "cube2::cube"
# Calculate the total contact wrench of cube3
assert cube3.get_link("cube").contact_wrench() == pytest.approx([0, 0, 50, 0, 0, 0],
abs=1.1)
# Calculate the total contact force of the cubes below.
# They will have 1.5 their weight from below and -0.5 from above.
assert cube1.get_link("cube").contact_wrench()[2] == pytest.approx(50, abs=1.1)
assert cube1.get_link("cube").contact_wrench()[2] == pytest.approx(50, abs=1.1)
# Check the normals and the sign of the forces
for contact in cube2.contacts():
if contact.body_b == "cube3::cube":
for point in contact.points:
assert point.force[2] < 0
assert point.normal == pytest.approx([0, 0, -1], abs=0.001)
if contact.body_b == "ground_plane::link":
for point in contact.points:
assert point.force[2] > 0
assert point.normal == pytest.approx([0, 0, 1], abs=0.001)