Merge pull request #980 from StanfordVL/fix/contact-checking

Address Contact Issues

omnigibson/action_primitives/starter_semantic_action_primitives.py

@@ -491,7 +491,7 @@ def apply(self, action):
         action = StarterSemanticActionPrimitiveSet(action_idx)
         return self.apply_ref(action, target_obj)
 
-    def apply_ref(self, prim, *args, attempts=3):
+    def apply_ref(self, prim, *args, attempts=5):
         """
         Yields action for robot to execute the primitive with the given arguments.
 

omnigibson/object_states/contact_bodies.py

@@ -4,6 +4,10 @@
 
 
 class ContactBodies(AbsoluteObjectState):
+    """
+    NOTE: This is slow and uncached, but it works even for sleeping objects.
+    For frequent contact checks, consider using RigidContactAPI for performance.
+    """
 
     def _get_value(self, ignore_objs=None):
         # Compute bodies in contact, minus the self-owned bodies

omnigibson/object_states/touching.py

@@ -2,7 +2,6 @@
 from omnigibson.object_states.kinematics_mixin import KinematicsMixin
 from omnigibson.object_states.object_state_base import BooleanStateMixin, RelativeObjectState
 from omnigibson.utils.constants import PrimType
-from omnigibson.utils.usd_utils import RigidContactAPI
 
 
 class Touching(KinematicsMixin, RelativeObjectState, BooleanStateMixin):
@@ -20,11 +19,5 @@ def _get_value(self, other):
             return self._check_contact(other, self.obj)
         elif other.prim_type == PrimType.CLOTH:
             return self._check_contact(self.obj, other)
-        # elif not self.obj.kinematic_only and not other.kinematic_only:
-        #     # Use optimized check for rigid bodies
-        #     return RigidContactAPI.in_contact(
-        #         prim_paths_a=[link.prim_path for link in self.obj.links.values()],
-        #         prim_paths_b=[link.prim_path for link in other.links.values()],
-        #     )
         else:
             return self._check_contact(other, self.obj) and self._check_contact(self.obj, other)

omnigibson/prims/entity_prim.py

@@ -21,7 +21,8 @@
 m = create_module_macros(module_path=__file__)
 
 # Default sleep threshold for all objects -- see https://docs.omniverse.nvidia.com/extensions/latest/ext_physics/simulation-control/physics-settings.html?highlight=sleep#sleeping
-m.DEFAULT_SLEEP_THRESHOLD = 0.001
+# Mass-normalized kinetic energy threshold below which an actor may go to sleep
+m.DEFAULT_SLEEP_THRESHOLD = 0.00005
 
 
 class EntityPrim(XFormPrim):
@@ -613,6 +614,8 @@ def visual_only(self, val):
     def contact_list(self):
         """
         Get list of all current contacts with this object prim
+        NOTE: This method is slow and uncached, but it works even for sleeping objects.
+        For frequent contact checks, consider using RigidContactAPI for performance.
 
         Returns:
             list of CsRawData: raw contact info for this rigid body

omnigibson/prims/rigid_prim.py

@@ -67,7 +67,6 @@ def __init__(
         # Other values that will be filled in at runtime
         self._rigid_prim_view_direct = None
         self._belongs_to_articulation = None
-        self._cs = None  # Contact sensor interface
         self._body_name = None
 
         self._visual_only = None
@@ -115,11 +114,12 @@ def _post_load(self):
 
         # Only create contact report api if we're not visual only
         if not self._visual_only:
-            (
+            contact_api = (
                 lazy.pxr.PhysxSchema.PhysxContactReportAPI(self._prim)
                 if self._prim.HasAPI(lazy.pxr.PhysxSchema.PhysxContactReportAPI)
                 else lazy.pxr.PhysxSchema.PhysxContactReportAPI.Apply(self._prim)
             )
+            contact_api.GetThresholdAttr().Set(0.0)
 
         # Store references to owned visual / collision meshes
         # We iterate over all children of this object's prim,
@@ -144,10 +144,6 @@ def _post_load(self):
             else False
         )
 
-        # Create contact sensor
-        self._cs = lazy.omni.isaac.sensor._sensor.acquire_contact_sensor_interface()
-        # self._create_contact_sensor()
-
     def _initialize(self):
         # Run super method first
         super()._initialize()
@@ -159,7 +155,7 @@ def _initialize(self):
 
         # Get contact info first
         if self.contact_reporting_enabled:
-            self._cs.get_rigid_body_raw_data(self.prim_path)
+            og.sim.contact_sensor.get_rigid_body_raw_data(self.prim_path)
 
         # Grab handle to this rigid body and get name
         self.update_handles()
@@ -261,19 +257,21 @@ def update_handles(self):
     def contact_list(self):
         """
         Get list of all current contacts with this rigid body
+        NOTE: This method is slow and uncached, but it works even for sleeping objects.
+        For frequent contact checks, consider using RigidContactAPI for performance.
 
         Returns:
             list of CsRawData: raw contact info for this rigid body
         """
         # Make sure we have the ability to grab contacts for this object
         contacts = []
         if self.contact_reporting_enabled:
-            raw_data = self._cs.get_rigid_body_raw_data(self.prim_path)
+            raw_data = og.sim.contact_sensor.get_rigid_body_raw_data(self.prim_path)
             for c in raw_data:
                 # convert handles to prim paths for comparison
                 c = [*c]  # CsRawData enforces body0 and body1 types to be ints, but we want strings
-                c[2] = self._cs.decode_body_name(c[2])
-                c[3] = self._cs.decode_body_name(c[3])
+                c[2] = og.sim.contact_sensor.decode_body_name(c[2])
+                c[3] = og.sim.contact_sensor.decode_body_name(c[3])
                 contacts.append(CsRawData(*c))
         return contacts
 

omnigibson/scenes/scene_base.py

@@ -456,6 +456,7 @@ def load(self, idx, **kwargs):
         self._pose_inv = th.linalg.inv_ex(self._pose).inverse
 
         if gm.ENABLE_TRANSITION_RULES:
+            assert gm.ENABLE_OBJECT_STATES, "Transition rules require object states to be enabled!"
             self._transition_rule_api = TransitionRuleAPI(scene=self)
 
         # Always stop the sim if we started it internally

omnigibson/simulator.py

@@ -403,6 +403,9 @@ def __init__(
                 self.viewer_width = viewer_width
                 self.viewer_height = viewer_height
 
+            # Acquire contact sensor interface
+            self._contact_sensor = lazy.omni.isaac.sensor._sensor.acquire_contact_sensor_interface()
+
         def _set_viewer_camera(self, relative_prim_path="/viewer_camera", viewport_name="Viewport"):
             """
             Creates a camera prim dedicated for this viewer at @prim_path if it doesn't exist,
@@ -1733,6 +1736,14 @@ def initial_rendering_dt(self):
             """
             return self._initial_rendering_dt
 
+        @property
+        def contact_sensor(self):
+            """
+            Returns:
+                ContactSensor: Contact sensor object
+            """
+            return self._contact_sensor
+
         def _dump_state(self):
             # Default state is from the scene
             return {i: scene.dump_state(serialized=False) for i, scene in enumerate(self.scenes)}

omnigibson/transition_rules.py

@@ -334,6 +334,10 @@ class TouchingAnyCondition(RuleCondition):
     """
     Rule condition that prunes object candidates from @filter_1_name, only keeping any that are touching any object
     from @filter_2_name
+
+    Note that this condition uses the RigidContactAPI for contact checking. This is not a persistent contact check,
+    meaning that if objects get in contact for some time and both fall asleep, the contact will not be detected.
+    To get persistent contact checking, please use contact_sensor.
     """
 
     def __init__(self, filter_1_name, filter_2_name):
@@ -354,71 +358,39 @@ def __init__(self, filter_1_name, filter_2_name):
         # If optimized, filter_2_idxs will be used, otherwise filter_2_bodies will be used!
         # Maps object to the list of rigid body idxs in the global contact matrix corresponding to filter 2
         self._filter_2_idxs = None
-        # Maps object to set of rigid bodies corresponding to filter 2
-        self._filter_2_bodies = None
-
-        # Flag whether optimized call can be used
-        self._optimized = None
 
     def refresh(self, object_candidates):
-        # Check whether we can use optimized computation or not -- this is determined by whether or not any objects
-        # in our collision set are kinematic only
-        # self._optimized = not th.any(
-        #     th.tensor(
-        #         [
-        #             obj.kinematic_only or obj.prim_type == PrimType.CLOTH
-        #             for f in (self._filter_1_name, self._filter_2_name)
-        #             for obj in object_candidates[f]
-        #         ]
-        #     )
-        # )
-
-        # TODO: RigidContactAPI sometimes has false negatives (returning zero impulses when there are contacts), so we will stick
-        # with the non-optimized version for now. We will fix this in a future release.
-        self._optimized = False
-
-        if self._optimized:
-            # Register idx mappings
-            self._filter_1_idxs = {
-                obj: [RigidContactAPI.get_body_row_idx(link.prim_path)[1] for link in obj.links.values()]
-                for obj in object_candidates[self._filter_1_name]
-            }
-            self._filter_2_idxs = {
-                obj: th.tensor(
-                    [RigidContactAPI.get_body_col_idx(link.prim_path)[1] for link in obj.links.values()],
-                    dtype=th.float32,
-                )
-                for obj in object_candidates[self._filter_2_name]
-            }
-        else:
-            # Register body mappings
-            self._filter_2_bodies = {obj: set(obj.links.values()) for obj in object_candidates[self._filter_2_name]}
+        # Register idx mappings
+        self._filter_1_idxs = {
+            obj: [RigidContactAPI.get_body_row_idx(link.prim_path)[1] for link in obj.links.values()]
+            for obj in object_candidates[self._filter_1_name]
+        }
+        self._filter_2_idxs = {
+            obj: th.tensor(
+                [RigidContactAPI.get_body_col_idx(link.prim_path)[1] for link in obj.links.values()],
+                dtype=th.float32,
+            )
+            for obj in object_candidates[self._filter_2_name]
+        }
 
     def __call__(self, object_candidates):
         # Keep any object that has non-zero impulses between itself and any of the @filter_2_name's objects
         objs = []
 
-        if self._optimized:
-            # Batch check for each object
-            for obj in object_candidates[self._filter_1_name]:
-                # Get all impulses between @obj and any object in @filter_2_name that are in the same scene
-                idxs_to_check = th.cat(
-                    [
-                        self._filter_2_idxs[obj2]
-                        for obj2 in object_candidates[self._filter_2_name]
-                        if obj2.scene == obj.scene
-                    ]
-                )
-                if th.any(
-                    RigidContactAPI.get_all_impulses(obj.scene.idx)[self._filter_1_idxs[obj]][:, idxs_to_check.tolist()]
-                ):
-                    objs.append(obj)
-        else:
-            # Manually check contact
-            filter_2_bodies = set.union(*(self._filter_2_bodies[obj] for obj in object_candidates[self._filter_2_name]))
-            for obj in object_candidates[self._filter_1_name]:
-                if len(obj.states[ContactBodies].get_value().intersection(filter_2_bodies)) > 0:
-                    objs.append(obj)
+        # Batch check for each object
+        for obj in object_candidates[self._filter_1_name]:
+            # Get all impulses between @obj and any object in @filter_2_name that are in the same scene
+            idxs_to_check = th.cat(
+                [
+                    self._filter_2_idxs[obj2]
+                    for obj2 in object_candidates[self._filter_2_name]
+                    if obj2.scene == obj.scene
+                ]
+            )
+            if th.any(
+                RigidContactAPI.get_all_impulses(obj.scene.idx)[self._filter_1_idxs[obj]][:, idxs_to_check.tolist()]
+            ):
+                objs.append(obj)
 
         # Update candidates
         object_candidates[self._filter_1_name] = objs

omnigibson/utils/usd_utils.py

@@ -185,6 +185,8 @@ def create_joint(
 class RigidContactAPIImpl:
     """
     Class containing class methods to aggregate rigid body contacts across all rigid bodies in the simulator
+
+    NOTE: The RigidContactAPI only works when the contacting objects are awake. If the objects could be asleep, use ContactBodies instead.
     """
 
     def __init__(self):
@@ -230,7 +232,7 @@ def get_column_filters(cls):
 
     @classmethod
     def get_max_contact_data_count(cls):
-        return 0
+        return 256
 
     def initialize_view(self):
         """

tests/test_robot_states_no_flatcache.py

@@ -40,5 +40,6 @@ def test_object_in_FOV_of_robot():
     vision_sensor.set_position_orientation(position=[100, 150, 100])
     og.sim.step()
     og.sim.step()
-    assert robot.states[ObjectsInFOVOfRobot].get_value() == [robot]
+    # Since the sensor is moved away from the robot, the robot should not see itself
+    assert robot.states[ObjectsInFOVOfRobot].get_value() == []
     og.clear()