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)