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_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 visualise_spawn_volume(self,
task: SupportedTasks,
gazebo: scenario.GazeboSimulator,
color: Tuple[float, float, float,
float] = (0, 0, 1, 0.8),
color_with_height: Tuple[float, float, float,
float] = (1, 0, 1,
0.7)):
# Insert translucent boxes visible only in simulation with no physical interactions
models.Box(world=task.world,
name="object_spawn_volume",
position=task._object_spawn_centre,
orientation=(0, 0, 0, 1),
size=task._object_spawn_volume,
collision=False,
visual=True,
gui_only=True,
static=True,
color=color)
models.Box(world=task.world,
name="object_spawn_volume_with_height",
position=task._object_spawn_centre,
orientation=(0, 0, 0, 1),
size=task._object_spawn_volume,
collision=False,
visual=True,
gui_only=True,
static=True,
color=color_with_height)
# Execute a paused run to process model insertion
if not gazebo.run(paused=True):
raise RuntimeError("Failed to execute a paused Gazebo run")
def add_robot(self, task: SupportedTasks,
gazebo: scenario.GazeboSimulator):
robot = None
if 'panda' == task._robot_model:
robot = models.Panda(
world=task.world,
position=task._robot_position,
orientation=conversions.Quaternion.to_wxyz(
task._robot_quat_xyzw),
arm_collision=task._robot_arm_collision,
hand_collision=task._robot_hand_collision,
initial_joint_positions=task._robot_initial_joint_positions)
task.robot_name = robot.name()
task.robot_base_link_name = robot.get_base_link_name()
task.robot_ee_link_name = robot.get_ee_link_name()
task.robot_gripper_link_names = robot.get_gripper_link_names()
# TODO (low priority): TF2 - Move this to task
robot_base_frame_id = robot.link_names()[0]
self._tf2_broadcaster.broadcast_tf(translation=task._robot_position,
rotation=task._robot_quat_xyzw,
xyzw=True,
child_frame_id=robot_base_frame_id)
# Execute a paused run to process model insertion
if not gazebo.run(paused=True):
raise RuntimeError("Failed to execute a paused Gazebo run")
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_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 add_default_ground(self, task: SupportedTasks,
gazebo: scenario.GazeboSimulator):
ground = models.Ground(world=task.world,
position=task._ground_position,
orientation=conversions.Quaternion.to_wxyz(
task._ground_quat_xyzw),
size=task._ground_size)
task.ground_name = ground.name()
# Execute a paused run to process model insertion
if not gazebo.run(paused=True):
raise RuntimeError("Failed to execute a paused Gazebo run")
def add_invisible_world_bottom_collision_plane(
self, task: SupportedTasks, gazebo: scenario.GazeboSimulator):
models.Plane(world=task.world,
position=(0.0, 0.0, -1.0),
orientation=(1.0, 0.0, 0.0, 0.0),
direction=(0.0, 0.0, 1.0),
collision=True,
friction=10.0,
visual=False)
# Execute a paused run to process model insertion
if not gazebo.run(paused=True):
raise RuntimeError("Failed to execute a paused Gazebo run")
def get_cube(gazebo: scenario.GazeboSimulator, world: scenario.World) -> core.Model:
quaternion = to_wxyz(Rotation.from_euler("x", 45, degrees=True).as_quat())
initial_pose = core.Pose([0, 0, 0.5], quaternion.tolist())
cube_urdf = utils.get_cube_urdf()
assert world.insert_model(cube_urdf, initial_pose)
assert "cube_robot" in world.model_names()
cube = world.get_model("cube_robot")
assert cube.to_gazebo().reset_base_world_linear_velocity([0.1, -0.2, -0.3])
assert cube.to_gazebo().reset_base_world_angular_velocity([-0.1, 2.0, 0.3])
assert gazebo.run(paused=True)
return cube
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 add_default_object(self, task: SupportedTasks,
gazebo: scenario.GazeboSimulator):
object_model = None
if 'box' == task._object_type:
object_model = models.Box(
world=task.world,
position=task._object_spawn_centre,
orientation=conversions.Quaternion.to_wxyz(
task._object_quat_xyzw),
size=task._object_dimensions,
mass=task._object_mass,
collision=task._object_collision,
visual=task._object_visual,
static=task._object_static,
color=task._object_color)
elif 'sphere' == task._object_type:
object_model = models.Sphere(
world=task.world,
position=task._object_spawn_centre,
orientation=conversions.Quaternion.to_wxyz(
task._object_quat_xyzw),
radius=task._object_dimensions[0],
mass=task._object_mass,
collision=task._object_collision,
visual=task._object_visual,
static=task._object_static,
color=task._object_color)
elif 'cylinder' == task._object_type:
object_model = models.Cylinder(
world=task.world,
position=task._object_spawn_centre,
orientation=conversions.Quaternion.to_wxyz(
task._object_quat_xyzw),
radius=task._object_dimensions[0],
length=task._object_dimensions[1],
mass=task._object_mass,
collision=task._object_collision,
visual=task._object_visual,
static=task._object_static,
color=task._object_color)
task.object_names.append(object_model.name())
# Execute a paused run to process model insertion
if not gazebo.run(paused=True):
raise RuntimeError("Failed to execute a paused Gazebo run")
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 add_camera(self,
task: SupportedTasks,
gazebo: scenario.GazeboSimulator):
# First add a sensor plugin to the world to enable
task.world.to_gazebo().insert_world_plugin("libignition-gazebo-sensors-system.so",
"ignition::gazebo::systems::Sensors",
f"""
<sdf version="1.7">
<render_engine>{task._camera_render_engine}</render_engine>
</sdf>
""")
# Create camera
camera = models.Camera(world=task.world,
position=task._camera_position,
orientation=conversions.Quaternion.to_wxyz(
task._camera_quat_xyzw),
camera_type=task._camera_type,
width=task._camera_width,
height=task._camera_height,
update_rate=task._camera_update_rate,
horizontal_fov=task._camera_horizontal_fov,
vertical_fov=task._camera_vertical_fov,
clip_color=task._camera_clip_color,
clip_depth=task._camera_clip_depth,
noise_mean=self._camera_noise_mean,
noise_stddev=self._camera_noise_stddev,
ros2_bridge_color=task._camera_ros2_bridge_color,
ros2_bridge_depth=task._camera_ros2_bridge_depth,
ros2_bridge_points=task._camera_ros2_bridge_points)
task.camera_name = camera.name()
# TODO (low priority): TF2 - Move this to task
camera_base_frame_id = camera.frame_id()
self._tf2_broadcaster.broadcast_tf(translation=task._camera_position,
rotation=task._camera_quat_xyzw,
xyzw=True,
child_frame_id=camera_base_frame_id)
# Execute a paused run to process model insertion
if not gazebo.run(paused=True):
raise RuntimeError("Failed to execute a paused Gazebo run")
def get_random_panda(gazebo: scenario.GazeboSimulator,
world: scenario.World) -> core.Model:
panda_urdf = gym_ignition_models.get_model_file("panda")
assert world.insert_model(panda_urdf)
assert "panda" in world.model_names()
panda = world.get_model("panda")
joint_space = get_joint_positions_space(model=panda)
joint_space.seed(10)
q = joint_space.sample()
dq = joint_space.np_random.uniform(low=-1.0, high=1.0, size=q.shape)
assert panda.to_gazebo().reset_joint_positions(q.tolist())
assert panda.to_gazebo().reset_joint_velocities(dq.tolist())
assert gazebo.run(paused=True)
return panda
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 randomize_models(self,
task: SupportedTasks,
gazebo: scenario.GazeboSimulator):
"""
Randomize models if needed.
"""
# Randomize robot joint positions if needed, else reset
if self.robot_joint_position_randomizer_enabled():
self.robot_random_joint_positions(task=task)
else:
self.reset_robot_joint_positions(task=task)
# Randomize ground plane if needed
if task._ground_enable and self.ground_model_expired():
self.randomize_ground(task=task)
# Randomize camera if needed
if task._camera_enable and self.camera_pose_expired():
self.randomize_camera_pose(task=task)
# Randomize objects if needed
# Note: No need to randomize pose of new models because they are already spawned randomly
self.__object_positions.clear()
if task._object_enable:
if self.object_models_expired():
if self._object_random_use_mesh_models:
self.randomize_object_models(task=task)
else:
self.randomize_object_primitives(task=task)
elif self.object_poses_randomizer_enabled():
self.object_random_pose(task=task)
elif not self.object_models_randomizer_enabled():
self.reset_default_object_pose(task=task)
# Execute a paused run to process these randomization operations
if not gazebo.run(paused=True):
raise RuntimeError("Failed to execute a paused Gazebo run")
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_run(gazebo: scenario.GazeboSimulator):
assert gazebo.initialize()
assert gazebo.run(paused=True)
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_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)
