Unverified Commit cb5c7cab authored by Mayank Mittal's avatar Mayank Mittal Committed by GitHub

Removes `disable_contact_processing` flag from SimulationContext (#1861)

# Description

The `disable_contact_processing` flag inside the SimulationContext
causes confusion, as we were setting it to True in many environments,
but it should be False since we want PhysX to report the contacts.

This MR changes the default behavior now. It is always disabled (i.e. no
contact reporting), until a contact sensor is created. It makes sure
that the performance isn't hampered in situations where the user doesn't
want to create any contact sensors.

## Type of Change

- Bug fix (non-breaking change which fixes an issue)
- Breaking change (fix or feature that would cause existing
functionality to not work as expected)

## Checklist

- [x] I have run the [`pre-commit` checks](https://pre-commit.com/) with
`./isaaclab.sh --format`
- [x] I have made corresponding changes to the documentation
- [ ] My changes generate no new warnings
- [ ] I have added tests that prove my fix is effective or that my
feature works
- [x] I have updated the changelog and the corresponding version in the
extension's `config/extension.toml` file
- [x] I have added my name to the `CONTRIBUTORS.md` or my name already
exists there
parent 3d836ab1
...@@ -70,53 +70,52 @@ depending on the complexity of the environment, the number of expected contacts ...@@ -70,53 +70,52 @@ depending on the complexity of the environment, the number of expected contacts
and the number of actors in the environment. The :class:`~isaaclab.sim.PhysxCfg` class provides access and the number of actors in the environment. The :class:`~isaaclab.sim.PhysxCfg` class provides access
for setting the GPU buffer dimensions. for setting the GPU buffer dimensions.
+--------------------------------------------------------------+-------------------------------------------------------------------+ +--------------------------------------------------+---------------------------------------------------------------+
| | | || || |
|.. code-block:: yaml |.. code-block:: python | || || |
| | | || # OmniIsaacGymEnvs || # IsaacLab |
| # OmniIsaacGymEnvs | # IsaacLab | || sim: || sim: SimulationCfg = SimulationCfg( |
| sim: | sim: SimulationCfg = SimulationCfg( | || || device = "cuda:0" # can be "cpu", "cuda", "cuda:<device_id>" |
| | device = "cuda:0" # can be "cpu", "cuda", "cuda:<device_id>" | || dt: 0.0083 # 1/120 s || dt=1 / 120, |
| dt: 0.0083 # 1/120 s | dt=1 / 120, | || use_gpu_pipeline: ${eq:${...pipeline},"gpu"} || # use_gpu_pipeline is deduced from the device |
| use_gpu_pipeline: ${eq:${...pipeline},"gpu"} | # use_gpu_pipeline is deduced from the device | || use_fabric: True || use_fabric=True, |
| use_fabric: True | use_fabric=True, | || enable_scene_query_support: False || enable_scene_query_support=False, |
| enable_scene_query_support: False | enable_scene_query_support=False, | || disable_contact_processing: False || |
| disable_contact_processing: False | disable_contact_processing=False, | || gravity: [0.0, 0.0, -9.81] || gravity=(0.0, 0.0, -9.81), |
| gravity: [0.0, 0.0, -9.81] | gravity=(0.0, 0.0, -9.81), | || || |
| | | || default_physics_material: || physics_material=RigidBodyMaterialCfg( |
| default_physics_material: | physics_material=RigidBodyMaterialCfg( | || static_friction: 1.0 || static_friction=1.0, |
| static_friction: 1.0 | static_friction=1.0, | || dynamic_friction: 1.0 || dynamic_friction=1.0, |
| dynamic_friction: 1.0 | dynamic_friction=1.0, | || restitution: 0.0 || restitution=0.0 |
| restitution: 0.0 | restitution=0.0 | || || ) |
| | ) | || physx: || physx: PhysxCfg = PhysxCfg( |
| physx: | physx: PhysxCfg = PhysxCfg( | || worker_thread_count: ${....num_threads} || # worker_thread_count is no longer needed |
| worker_thread_count: ${....num_threads} | # worker_thread_count is no longer needed | || solver_type: ${....solver_type} || solver_type=1, |
| solver_type: ${....solver_type} | solver_type=1, | || use_gpu: ${contains:"cuda",${....sim_device}} || # use_gpu is deduced from the device |
| use_gpu: ${contains:"cuda",${....sim_device}} | # use_gpu is deduced from the device | || solver_position_iteration_count: 4 || max_position_iteration_count=4, |
| solver_position_iteration_count: 4 | max_position_iteration_count=4, | || solver_velocity_iteration_count: 0 || max_velocity_iteration_count=0, |
| solver_velocity_iteration_count: 0 | max_velocity_iteration_count=0, | || contact_offset: 0.02 || # moved to actor config |
| contact_offset: 0.02 | # moved to actor config | || rest_offset: 0.001 || # moved to actor config |
| rest_offset: 0.001 | # moved to actor config | || bounce_threshold_velocity: 0.2 || bounce_threshold_velocity=0.2, |
| bounce_threshold_velocity: 0.2 | bounce_threshold_velocity=0.2, | || friction_offset_threshold: 0.04 || friction_offset_threshold=0.04, |
| friction_offset_threshold: 0.04 | friction_offset_threshold=0.04, | || friction_correlation_distance: 0.025 || friction_correlation_distance=0.025, |
| friction_correlation_distance: 0.025 | friction_correlation_distance=0.025, | || enable_sleeping: True || # enable_sleeping is no longer needed |
| enable_sleeping: True | # enable_sleeping is no longer needed | || enable_stabilization: True || enable_stabilization=True, |
| enable_stabilization: True | enable_stabilization=True, | || max_depenetration_velocity: 100.0 || # moved to RigidBodyPropertiesCfg |
| max_depenetration_velocity: 100.0 | # moved to RigidBodyPropertiesCfg | || || |
| | | || gpu_max_rigid_contact_count: 524288 || gpu_max_rigid_contact_count=2**23, |
| gpu_max_rigid_contact_count: 524288 | gpu_max_rigid_contact_count=2**23, | || gpu_max_rigid_patch_count: 81920 || gpu_max_rigid_patch_count=5 * 2**15, |
| gpu_max_rigid_patch_count: 81920 | gpu_max_rigid_patch_count=5 * 2**15, | || gpu_found_lost_pairs_capacity: 1024 || gpu_found_lost_pairs_capacity=2**21, |
| gpu_found_lost_pairs_capacity: 1024 | gpu_found_lost_pairs_capacity=2**21, | || gpu_found_lost_aggregate_pairs_capacity: 262144 || gpu_found_lost_aggregate_pairs_capacity=2**25, |
| gpu_found_lost_aggregate_pairs_capacity: 262144 | gpu_found_lost_aggregate_pairs_capacity=2**25, | || gpu_total_aggregate_pairs_capacity: 1024 || gpu_total_aggregate_pairs_capacity=2**21, |
| gpu_total_aggregate_pairs_capacity: 1024 | gpu_total_aggregate_pairs_capacity=2**21, | || gpu_heap_capacity: 67108864 || gpu_heap_capacity=2**26, |
| gpu_heap_capacity: 67108864 | gpu_heap_capacity=2**26, | || gpu_temp_buffer_capacity: 16777216 || gpu_temp_buffer_capacity=2**24, |
| gpu_temp_buffer_capacity: 16777216 | gpu_temp_buffer_capacity=2**24, | || gpu_max_num_partitions: 8 || gpu_max_num_partitions=8, |
| gpu_max_num_partitions: 8 | gpu_max_num_partitions=8, | || gpu_max_soft_body_contacts: 1048576 || gpu_max_soft_body_contacts=2**20, |
| gpu_max_soft_body_contacts: 1048576 | gpu_max_soft_body_contacts=2**20, | || gpu_max_particle_contacts: 1048576 || gpu_max_particle_contacts=2**20, |
| gpu_max_particle_contacts: 1048576 | gpu_max_particle_contacts=2**20, | || || ) |
| | ) | || || ) |
| | ) | +--------------------------------------------------+---------------------------------------------------------------+
+--------------------------------------------------------------+-------------------------------------------------------------------+
Parameters such as ``add_ground_plane`` and ``add_distant_light`` are now part of the task logic when creating the scene. Parameters such as ``add_ground_plane`` and ``add_distant_light`` are now part of the task logic when creating the scene.
``enable_cameras`` is now a command line argument ``--enable_cameras`` that can be passed directly to the training script. ``enable_cameras`` is now a command line argument ``--enable_cameras`` that can be passed directly to the training script.
......
[package] [package]
# Note: Semantic Versioning is used: https://semver.org/ # Note: Semantic Versioning is used: https://semver.org/
version = "0.34.4" version = "0.34.5"
# Description # Description
title = "Isaac Lab framework for Robot Learning" title = "Isaac Lab framework for Robot Learning"
......
Changelog Changelog
--------- ---------
0.34.5 (2025-03-02)
~~~~~~~~~~~~~~~~~~~
Changed
^^^^^^^
* Enabled the physics flag for disabling contact processing in the :class:`~isaaclab.sim.SimulationContact`
class. This means that by default, no contact reporting is done by the physics engine, which should provide
a performance boost in simulations with no contact processing requirements.
* Disabled the physics flag for disabling contact processing in the :class:`~isaaclab.sensors.ContactSensor`
class when the sensor is created to allow contact reporting for the sensor.
Removed
^^^^^^^
* Removed the attribute ``disable_contact_processing`` from :class:`~isaaclab.sim.SimulationContact`.
0.34.4 (2025-03-01) 0.34.4 (2025-03-01)
~~~~~~~~~~~~~~~~~~~ ~~~~~~~~~~~~~~~~~~~
...@@ -136,7 +154,8 @@ Added ...@@ -136,7 +154,8 @@ Added
Changed Changed
^^^^^^^ ^^^^^^^
* Automatic collision filtering now happens as part of the replicate_physics call. When replicate_physics is not enabled, we call the previous ``filter_collisions`` API to mask collisions between environments. * Automatic collision filtering now happens as part of the replicate_physics call. When replicate_physics is not enabled, we call the previous
``filter_collisions`` API to mask collisions between environments.
0.33.10 (2025-01-22) 0.33.10 (2025-01-22)
...@@ -145,7 +164,11 @@ Changed ...@@ -145,7 +164,11 @@ Changed
Changed Changed
^^^^^^^ ^^^^^^^
* In :meth:`isaaclab.assets.Articulation.write_joint_limits_to_sim`, we previously added a check for if default joint positions exceed the new limits being set. When this is True, we log a warning message to indicate that the default joint positions will be clipped to be within the range of the new limits. However, the warning message can become overly verbose in a randomization setting where this API is called on every environment reset. We now default to only writing the message to info level logging if called within randomization, and expose a parameter that can be used to choose the logging level desired. * In :meth:`isaaclab.assets.Articulation.write_joint_limits_to_sim`, we previously added a check for if default joint positions exceed the
new limits being set. When this is True, we log a warning message to indicate that the default joint positions will be clipped to be within
the range of the new limits. However, the warning message can become overly verbose in a randomization setting where this API is called on
every environment reset. We now default to only writing the message to info level logging if called within randomization, and expose a
parameter that can be used to choose the logging level desired.
0.33.9 (2025-01-22) 0.33.9 (2025-01-22)
...@@ -193,7 +216,9 @@ Fixed ...@@ -193,7 +216,9 @@ Fixed
Fixed Fixed
^^^^^ ^^^^^
* Fixed the respawn of only wrong object samples in :func:`repeated_objects_terrain` of :mod:`isaaclab.terrains.trimesh` module. Previously, the function was respawning all objects in the scene instead of only the wrong object samples, which in worst case could lead to infinite respawn loop. * Fixed the respawn of only wrong object samples in :func:`repeated_objects_terrain` of :mod:`isaaclab.terrains.trimesh` module.
Previously, the function was respawning all objects in the scene instead of only the wrong object samples, which in worst case
could lead to infinite respawn loop.
0.33.6 (2025-01-16) 0.33.6 (2025-01-16)
...@@ -314,7 +339,8 @@ Added ...@@ -314,7 +339,8 @@ Added
Fixed Fixed
^^^^^ ^^^^^
* Fixed ordering of logging and resamping in the command manager, where we were logging the metrics after resampling the commands. This leads to incorrect logging of metrics when inside the resample call, the metrics tensors get reset. * Fixed ordering of logging and resamping in the command manager, where we were logging the metrics after resampling the commands.
This leads to incorrect logging of metrics when inside the resample call, the metrics tensors get reset.
0.30.2 (2024-12-16) 0.30.2 (2024-12-16)
...@@ -340,7 +366,8 @@ Added ...@@ -340,7 +366,8 @@ Added
Changed Changed
^^^^^^^ ^^^^^^^
* Added call to update articulation kinematics after reset to ensure states are updated for non-rendering sensors. Previously, some changes in reset such as modifying joint states would not be reflected in the rigid body states immediately after reset. * Added call to update articulation kinematics after reset to ensure states are updated for non-rendering sensors. Previously, some changes
in reset such as modifying joint states would not be reflected in the rigid body states immediately after reset.
0.30.0 (2024-12-15) 0.30.0 (2024-12-15)
...@@ -351,9 +378,12 @@ Added ...@@ -351,9 +378,12 @@ Added
* Added UI interface to the Managers in the ManagerBasedEnv and MangerBasedRLEnv classes. * Added UI interface to the Managers in the ManagerBasedEnv and MangerBasedRLEnv classes.
* Added UI widgets for :class:`LiveLinePlot` and :class:`ImagePlot`. * Added UI widgets for :class:`LiveLinePlot` and :class:`ImagePlot`.
* Added ``ManagerLiveVisualizer/Cfg``: Given a ManagerBase (i.e. action_manager, observation_manager, etc) and a config file this class creates the the interface between managers and the UI. * Added ``ManagerLiveVisualizer/Cfg``: Given a ManagerBase (i.e. action_manager, observation_manager, etc) and a config file this class creates
* Added :class:`EnvLiveVisualizer`: A 'manager' of ManagerLiveVisualizer. This is added to the ManagerBasedEnv but is only called during the initialization of the managers in load_managers the the interface between managers and the UI.
* Added ``get_active_iterable_terms`` implementation methods to ActionManager, ObservationManager, CommandsManager, CurriculumManager, RewardManager, and TerminationManager. This method exports the active term data and labels for each manager and is called by ManagerLiveVisualizer. * Added :class:`EnvLiveVisualizer`: A 'manager' of ManagerLiveVisualizer. This is added to the ManagerBasedEnv but is only called during
the initialization of the managers in load_managers
* Added ``get_active_iterable_terms`` implementation methods to ActionManager, ObservationManager, CommandsManager, CurriculumManager,
RewardManager, and TerminationManager. This method exports the active term data and labels for each manager and is called by ManagerLiveVisualizer.
* Additions to :class:`BaseEnvWindow` and :class:`RLEnvWindow` to register ManagerLiveVisualizer UI interfaces for the chosen managers. * Additions to :class:`BaseEnvWindow` and :class:`RLEnvWindow` to register ManagerLiveVisualizer UI interfaces for the chosen managers.
...@@ -393,7 +423,9 @@ Changed ...@@ -393,7 +423,9 @@ Changed
Fixed Fixed
^^^^^ ^^^^^
* Fixed the shape of ``quat_w`` in the ``apply_actions`` method of :attr:`~isaaclab.env.mdp.NonHolonomicAction` (previously (N,B,4), now (N,4) since the number of root bodies B is required to be 1). Previously ``apply_actions`` errored because ``euler_xyz_from_quat`` requires inputs of shape (N,4). * Fixed the shape of ``quat_w`` in the ``apply_actions`` method of :attr:`~isaaclab.env.mdp.NonHolonomicAction`
(previously (N,B,4), now (N,4) since the number of root bodies B is required to be 1). Previously ``apply_actions`` errored
because ``euler_xyz_from_quat`` requires inputs of shape (N,4).
0.28.1 (2024-12-13) 0.28.1 (2024-12-13)
...@@ -2133,7 +2165,8 @@ Added ...@@ -2133,7 +2165,8 @@ Added
Fixed Fixed
^^^^^ ^^^^^
* Fixes the order of size arguments in :meth:`isaaclab.terrains.height_field.random_uniform_terrain`. Previously, the function would crash if the size along x and y were not the same. * Fixes the order of size arguments in :meth:`isaaclab.terrains.height_field.random_uniform_terrain`. Previously, the function
would crash if the size along x and y were not the same.
0.10.22 (2024-02-14) 0.10.22 (2024-02-14)
......
...@@ -12,6 +12,7 @@ import torch ...@@ -12,6 +12,7 @@ import torch
from collections.abc import Sequence from collections.abc import Sequence
from typing import TYPE_CHECKING from typing import TYPE_CHECKING
import carb
import omni.physics.tensors.impl.api as physx import omni.physics.tensors.impl.api as physx
from pxr import PhysxSchema from pxr import PhysxSchema
...@@ -70,6 +71,11 @@ class ContactSensor(SensorBase): ...@@ -70,6 +71,11 @@ class ContactSensor(SensorBase):
""" """
# initialize base class # initialize base class
super().__init__(cfg) super().__init__(cfg)
# Enable contact processing
carb_settings_iface = carb.settings.get_settings()
carb_settings_iface.set_bool("/physics/disableContactProcessing", False)
# Create empty variables for storing output data # Create empty variables for storing output data
self._data: ContactSensorData = ContactSensorData() self._data: ContactSensorData = ContactSensorData()
# initialize self._body_physx_view for running in extension mode # initialize self._body_physx_view for running in extension mode
......
...@@ -266,18 +266,6 @@ class SimulationCfg: ...@@ -266,18 +266,6 @@ class SimulationCfg:
updates, please set this flag to :obj:`False`. updates, please set this flag to :obj:`False`.
""" """
disable_contact_processing: bool = False
"""Enable/disable contact processing. Default is False.
By default, the physics engine processes all the contacts in the scene. However, reporting this contact
information can be expensive due to its combinatorial complexity. This flag allows disabling the contact
processing and querying the contacts manually by the user over a limited set of primitives in the scene.
.. note::
It is required to set this flag to :obj:`True` when using the TensorAPIs for contact reporting.
"""
physx: PhysxCfg = PhysxCfg() physx: PhysxCfg = PhysxCfg()
"""PhysX solver settings. Default is PhysxCfg().""" """PhysX solver settings. Default is PhysxCfg()."""
......
...@@ -130,11 +130,19 @@ class SimulationContext(_SimulationContext): ...@@ -130,11 +130,19 @@ class SimulationContext(_SimulationContext):
# change dispatcher to use the default dispatcher in PhysX SDK instead of carb tasking # change dispatcher to use the default dispatcher in PhysX SDK instead of carb tasking
# note: dispatcher handles how threads are launched for multi-threaded physics # note: dispatcher handles how threads are launched for multi-threaded physics
carb_settings_iface.set_bool("/physics/physxDispatcher", True) carb_settings_iface.set_bool("/physics/physxDispatcher", True)
# disable contact processing in omni.physx if requested # disable contact processing in omni.physx
# note: helpful when creating contact reporting over limited number of objects in the scene # note: we disable it by default to avoid the overhead of contact processing when it isn't needed.
if self.cfg.disable_contact_processing: # The physics flag gets enabled when a contact sensor is created.
carb_settings_iface.set_bool("/physics/disableContactProcessing", True) if hasattr(self.cfg, "disable_contact_processing"):
# enable custom geometry for cylinder and cone collision shapes to allow contact reporting for them omni.log.warn(
"The `disable_contact_processing` attribute is deprecated and always set to True"
" to avoid unnecessary overhead. Contact processing is automatically enabled when"
" a contact sensor is created, so manual configuration is no longer required."
)
# FIXME: From investigation, it seems this flag only affects CPU physics. For GPU physics, contacts
# are always processed. The issue is reported to the PhysX team by @mmittal.
carb_settings_iface.set_bool("/physics/disableContactProcessing", True)
# disable custom geometry for cylinder and cone collision shapes to allow contact reporting for them
# reason: cylinders and cones aren't natively supported by PhysX so we need to use custom geometry flags # reason: cylinders and cones aren't natively supported by PhysX so we need to use custom geometry flags
# reference: https://nvidia-omniverse.github.io/PhysX/physx/5.4.1/docs/Geometry.html?highlight=capsule#geometry # reference: https://nvidia-omniverse.github.io/PhysX/physx/5.4.1/docs/Geometry.html?highlight=capsule#geometry
carb_settings_iface.set_bool("/physics/collisionConeCustomGeometry", False) carb_settings_iface.set_bool("/physics/collisionConeCustomGeometry", False)
......
...@@ -20,6 +20,8 @@ import unittest ...@@ -20,6 +20,8 @@ import unittest
from dataclasses import MISSING from dataclasses import MISSING
from enum import Enum from enum import Enum
import carb
import isaaclab.sim as sim_utils import isaaclab.sim as sim_utils
from isaaclab.assets import RigidObject, RigidObjectCfg from isaaclab.assets import RigidObject, RigidObjectCfg
from isaaclab.scene import InteractiveScene, InteractiveSceneCfg from isaaclab.scene import InteractiveScene, InteractiveSceneCfg
...@@ -224,14 +226,25 @@ class TestContactSensor(unittest.TestCase): ...@@ -224,14 +226,25 @@ class TestContactSensor(unittest.TestCase):
cls.durations = [cls.sim_dt, cls.sim_dt * 2, cls.sim_dt * 32, cls.sim_dt * 128] cls.durations = [cls.sim_dt, cls.sim_dt * 2, cls.sim_dt * 32, cls.sim_dt * 128]
cls.terrains = [FLAT_TERRAIN_CFG, COBBLESTONE_TERRAIN_CFG] cls.terrains = [FLAT_TERRAIN_CFG, COBBLESTONE_TERRAIN_CFG]
cls.devices = ["cuda:0", "cpu"] cls.devices = ["cuda:0", "cpu"]
cls.carb_settings_iface = carb.settings.get_settings()
def test_cube_contact_time(self): def test_cube_contact_time(self):
"""Checks contact sensor values for contact time and air time for a cube collision primitive.""" """Checks contact sensor values for contact time and air time for a cube collision primitive."""
self._run_contact_sensor_test(shape_cfg=CUBE_CFG) # check for both contact processing enabled and disabled
# internally, the contact sensor should enable contact processing so it should always work.
for disable_contact_processing in [True, False]:
with self.subTest(disable_contact_processing=disable_contact_processing):
self.carb_settings_iface.set_bool("/physics/disableContactProcessing", disable_contact_processing)
self._run_contact_sensor_test(shape_cfg=CUBE_CFG)
def test_sphere_contact_time(self): def test_sphere_contact_time(self):
"""Checks contact sensor values for contact time and air time for a sphere collision primitive.""" """Checks contact sensor values for contact time and air time for a sphere collision primitive."""
self._run_contact_sensor_test(shape_cfg=SPHERE_CFG) # check for both contact processing enabled and disabled
# internally, the contact sensor should enable contact processing so it should always work.
for disable_contact_processing in [True, False]:
with self.subTest(disable_contact_processing=disable_contact_processing):
self.carb_settings_iface.set_bool("/physics/disableContactProcessing", disable_contact_processing)
self._run_contact_sensor_test(shape_cfg=SPHERE_CFG)
def test_cube_stack_contact_filtering(self): def test_cube_stack_contact_filtering(self):
"""Checks contact sensor reporting for filtering stacked cube prims.""" """Checks contact sensor reporting for filtering stacked cube prims."""
...@@ -271,6 +284,9 @@ class TestContactSensor(unittest.TestCase): ...@@ -271,6 +284,9 @@ class TestContactSensor(unittest.TestCase):
self.sim = sim self.sim = sim
self.scene = scene self.scene = scene
# Check that contact processing is enabled
self.assertFalse(self.carb_settings_iface.get("/physics/disableContactProcessing"))
# Play the simulation # Play the simulation
self.sim.reset() self.sim.reset()
...@@ -284,16 +300,86 @@ class TestContactSensor(unittest.TestCase): ...@@ -284,16 +300,86 @@ class TestContactSensor(unittest.TestCase):
# Reset the contact sensors # Reset the contact sensors
self.scene.reset() self.scene.reset()
# Let the scene come to a rest # Let the scene come to a rest
for _ in range(20): for _ in range(500):
self._perform_sim_step() self._perform_sim_step()
# Check values for cube 2 # Check values for cube 2 --> cube 1 is the only collision for cube 2
torch.testing.assert_close( torch.testing.assert_close(
contact_sensor_2.data.force_matrix_w[:, :, 0], contact_sensor_2.data.net_forces_w contact_sensor_2.data.force_matrix_w[:, :, 0], contact_sensor_2.data.net_forces_w
) )
# Check that forces are opposite and equal
torch.testing.assert_close( torch.testing.assert_close(
contact_sensor_2.data.force_matrix_w[:, :, 0], contact_sensor.data.force_matrix_w[:, :, 0] contact_sensor_2.data.force_matrix_w[:, :, 0], -contact_sensor.data.force_matrix_w[:, :, 0]
) )
# Check values are non-zero (contacts are happening and are getting reported)
self.assertGreater(contact_sensor_2.data.net_forces_w.sum().item(), 0.0)
self.assertGreater(contact_sensor.data.net_forces_w.sum().item(), 0.0)
def test_no_contact_reporting(self):
"""Test that forcing the disable of contact processing results in no contact reporting.
We borrow the test :func:`test_cube_stack_contact_filtering` to test this and force disable contact processing.
"""
# TODO: This test only works on CPU. For GPU, it seems the contact processing is not disabled.
for device in ["cpu"]:
with self.subTest(device=device):
with build_simulation_context(device=device, dt=self.sim_dt, add_lighting=True) as sim:
sim._app_control_on_stop_handle = None
# Instance new scene for the current terrain and contact prim.
scene_cfg = ContactSensorSceneCfg(num_envs=32, env_spacing=1.0, lazy_sensor_update=False)
scene_cfg.terrain = FLAT_TERRAIN_CFG.replace(prim_path="/World/ground")
# -- cube 1
scene_cfg.shape = CUBE_CFG.replace(prim_path="{ENV_REGEX_NS}/Cube_1")
scene_cfg.shape.init_state.pos = (0, -1.0, 1.0)
# -- cube 2 (on top of cube 1)
scene_cfg.shape_2 = CUBE_CFG.replace(prim_path="{ENV_REGEX_NS}/Cube_2")
scene_cfg.shape_2.init_state.pos = (0, -1.0, 1.525)
# -- contact sensor 1
scene_cfg.contact_sensor = ContactSensorCfg(
prim_path="{ENV_REGEX_NS}/Cube_1",
track_pose=True,
debug_vis=False,
update_period=0.0,
filter_prim_paths_expr=["{ENV_REGEX_NS}/Cube_2"],
)
# -- contact sensor 2
scene_cfg.contact_sensor_2 = ContactSensorCfg(
prim_path="{ENV_REGEX_NS}/Cube_2",
track_pose=True,
debug_vis=False,
update_period=0.0,
filter_prim_paths_expr=["{ENV_REGEX_NS}/Cube_1"],
)
scene = InteractiveScene(scene_cfg)
# Force disable contact processing
self.carb_settings_iface.set_bool("/physics/disableContactProcessing", True)
# Set variables internally for reference
self.sim = sim
self.scene = scene
# Play the simulation
self.sim.reset()
# Extract from scene for type hinting
contact_sensor: ContactSensor = self.scene["contact_sensor"]
contact_sensor_2: ContactSensor = self.scene["contact_sensor_2"]
# Check buffers have the right size
self.assertEqual(contact_sensor.contact_physx_view.filter_count, 1)
self.assertEqual(contact_sensor_2.contact_physx_view.filter_count, 1)
# Reset the contact sensors
self.scene.reset()
# Let the scene come to a rest
for _ in range(500):
self._perform_sim_step()
# check values are zero (contacts are happening but not reported)
self.assertEqual(contact_sensor.data.net_forces_w.sum().item(), 0.0)
self.assertEqual(contact_sensor.data.force_matrix_w.sum().item(), 0.0)
self.assertEqual(contact_sensor_2.data.net_forces_w.sum().item(), 0.0)
self.assertEqual(contact_sensor_2.data.force_matrix_w.sum().item(), 0.0)
def test_sensor_print(self): def test_sensor_print(self):
"""Test sensor print is working correctly.""" """Test sensor print is working correctly."""
...@@ -353,6 +439,9 @@ class TestContactSensor(unittest.TestCase): ...@@ -353,6 +439,9 @@ class TestContactSensor(unittest.TestCase):
self.sim = sim self.sim = sim
self.scene = scene self.scene = scene
# Check that contact processing is enabled
self.assertFalse(self.carb_settings_iface.get("/physics/disableContactProcessing"))
# Play the simulation # Play the simulation
self.sim.reset() self.sim.reset()
......
...@@ -63,7 +63,6 @@ class AnymalCFlatEnvCfg(DirectRLEnvCfg): ...@@ -63,7 +63,6 @@ class AnymalCFlatEnvCfg(DirectRLEnvCfg):
sim: SimulationCfg = SimulationCfg( sim: SimulationCfg = SimulationCfg(
dt=1 / 200, dt=1 / 200,
render_interval=decimation, render_interval=decimation,
disable_contact_processing=True,
physics_material=sim_utils.RigidBodyMaterialCfg( physics_material=sim_utils.RigidBodyMaterialCfg(
friction_combine_mode="multiply", friction_combine_mode="multiply",
restitution_combine_mode="multiply", restitution_combine_mode="multiply",
......
...@@ -36,7 +36,6 @@ class FrankaCabinetEnvCfg(DirectRLEnvCfg): ...@@ -36,7 +36,6 @@ class FrankaCabinetEnvCfg(DirectRLEnvCfg):
sim: SimulationCfg = SimulationCfg( sim: SimulationCfg = SimulationCfg(
dt=1 / 120, dt=1 / 120,
render_interval=decimation, render_interval=decimation,
disable_contact_processing=True,
physics_material=sim_utils.RigidBodyMaterialCfg( physics_material=sim_utils.RigidBodyMaterialCfg(
friction_combine_mode="multiply", friction_combine_mode="multiply",
restitution_combine_mode="multiply", restitution_combine_mode="multiply",
......
...@@ -62,7 +62,6 @@ class QuadcopterEnvCfg(DirectRLEnvCfg): ...@@ -62,7 +62,6 @@ class QuadcopterEnvCfg(DirectRLEnvCfg):
sim: SimulationCfg = SimulationCfg( sim: SimulationCfg = SimulationCfg(
dt=1 / 100, dt=1 / 100,
render_interval=decimation, render_interval=decimation,
disable_contact_processing=True,
physics_material=sim_utils.RigidBodyMaterialCfg( physics_material=sim_utils.RigidBodyMaterialCfg(
friction_combine_mode="multiply", friction_combine_mode="multiply",
restitution_combine_mode="multiply", restitution_combine_mode="multiply",
......
...@@ -319,7 +319,6 @@ class SpotFlatEnvCfg(LocomotionVelocityRoughEnvCfg): ...@@ -319,7 +319,6 @@ class SpotFlatEnvCfg(LocomotionVelocityRoughEnvCfg):
# simulation settings # simulation settings
self.sim.dt = 0.002 # 500 Hz self.sim.dt = 0.002 # 500 Hz
self.sim.render_interval = self.decimation self.sim.render_interval = self.decimation
self.sim.disable_contact_processing = True
self.sim.physics_material.static_friction = 1.0 self.sim.physics_material.static_friction = 1.0
self.sim.physics_material.dynamic_friction = 1.0 self.sim.physics_material.dynamic_friction = 1.0
self.sim.physics_material.friction_combine_mode = "multiply" self.sim.physics_material.friction_combine_mode = "multiply"
......
...@@ -301,7 +301,6 @@ class LocomotionVelocityRoughEnvCfg(ManagerBasedRLEnvCfg): ...@@ -301,7 +301,6 @@ class LocomotionVelocityRoughEnvCfg(ManagerBasedRLEnvCfg):
# simulation settings # simulation settings
self.sim.dt = 0.005 self.sim.dt = 0.005
self.sim.render_interval = self.decimation self.sim.render_interval = self.decimation
self.sim.disable_contact_processing = True
self.sim.physics_material = self.scene.terrain.physics_material self.sim.physics_material = self.scene.terrain.physics_material
self.sim.physx.gpu_max_rigid_patch_count = 10 * 2**15 self.sim.physx.gpu_max_rigid_patch_count = 10 * 2**15
# update sensor update periods # update sensor update periods
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment