Trigger volumes

omnigibson/examples/object_states/particle_applier_remover_demo.py

@@ -156,7 +156,7 @@ def main(random_selection=False, headless=False, short_exec=False):
     # Set the modifier object to be in position to modify particles
     if modifier_type == "particleRemover" and method_type == "Projection":
         tool.set_position_orientation(
-            position=[0, 0.3, 1.45],
+            position=[0, 0.3, 1.42],
             orientation=[0, 0, 0, 1.0],
         )
     elif modifier_type == "particleRemover" and method_type == "Adjacency":

omnigibson/object_states/particle_modifier.py

@@ -16,6 +16,7 @@
 from omnigibson.object_states.object_state_base import IntrinsicObjectState
 from omnigibson.object_states.saturated import ModifiedParticles, Saturated
 from omnigibson.object_states.toggle import ToggledOn
+from omnigibson.object_states.trigger_volume_colliders import TriggerVolumeColliders
 from omnigibson.object_states.update_state_mixin import UpdateStateMixin
 from omnigibson.prims.geom_prim import VisualGeomPrim
 from omnigibson.prims.prim_base import BasePrim
@@ -349,7 +350,8 @@ def overlap_callback(hit):
                 "height": 1.0,
                 "size": 1.0,
             }
-            mesh_prim_path = f"{self.link.prim_path}/mesh_0"
+            mesh_name = "mesh_0"
+            mesh_prim_path = f"{self.link.prim_path}/{mesh_name}"
 
             # Create a primitive shape if it doesn't already exist
             pre_existing_mesh = lazy.omni.isaac.core.utils.prims.get_prim_at_path(mesh_prim_path)
@@ -384,14 +386,13 @@ def overlap_callback(hit):
                         f"pre-existing mesh type ({mesh_type})"
                     )
 
-            # Create the visual geom instance referencing the generated mesh prim, and then hide it
-            self.projection_mesh = VisualGeomPrim(
-                relative_prim_path=absolute_prim_path_to_scene_relative(self.obj.scene, mesh_prim_path),
-                name=f"{name_prefix}_projection_mesh",
-            )
-            self.projection_mesh.load(self.obj.scene)
-            self.projection_mesh.initialize()
-            self.projection_mesh.visible = False
+            # Make sure the object updates its meshes, and assert that there's only a single visual mesh
+            self.link.update_meshes(trigger_mesh_paths=[mesh_prim_path])
+
+            assert (
+                len(self.link.visual_meshes) == 1
+            ), f"Expected only a single projection mesh for {self.link}, got: {len(self.link.visual_meshes)}"
+            self.projection_mesh = self.link.visual_meshes[mesh_name]
 
             # Make sure the shape-based attributes are not set, and only the scaling is set
             property_names = set(self.projection_mesh.prim.GetPropertyNames())
@@ -402,41 +403,34 @@ def overlap_callback(hit):
                         val == default_val
                     ), f"Projection mesh should have shape-based attribute {shape_attr} == {default_val}! Got: {val}"
 
-            # Set the scale based on projection mesh params
-            self.projection_mesh.scale = self._projection_mesh_params["extents"]
-
-            # Make sure the object updates its meshes, and assert that there's only a single visual mesh
-            self.link.update_meshes()
-            assert (
-                len(self.link.visual_meshes) == 1
-            ), f"Expected only a single projection mesh for {self.link}, got: {len(self.link.visual_meshes)}"
-
-            # Make sure the mesh is translated so that its tip lies at the metalink origin, and rotated so the vector
-            # from tip to tail faces the positive x axis
-            z_offset = (
-                0.0
-                if self._projection_mesh_params["type"] == "Sphere"
-                else self._projection_mesh_params["extents"][2] / 2
-            )
+            # If we just added this mesh, make some additional adjustments
+            if not pre_existing_mesh:
+                # Set the scale based on projection mesh params
+                self.projection_mesh.scale = self._projection_mesh_params["extents"]
+
+                # Make sure the mesh is translated so that its tip lies at the metalink origin, and rotated so the vector
+                # from tip to tail faces the positive x axis
+                z_offset = (
+                    0.0
+                    if self._projection_mesh_params["type"] == "Sphere"
+                    else self._projection_mesh_params["extents"][2] / 2
+                )
 
-            self.projection_mesh.set_position_orientation(
-                position=th.tensor([0, 0, -z_offset]),
-                orientation=T.euler2quat(th.tensor([0, 0, 0], dtype=th.float32)),
-                frame="parent",
-            )
+                self.projection_mesh.set_position_orientation(
+                    position=th.tensor([0, 0, -z_offset]),
+                    orientation=T.euler2quat(th.tensor([0, 0, 0], dtype=th.float32)),
+                    frame="parent",
+                )
 
             # Generate the function for checking whether points are within the projection mesh
             self._check_in_mesh, _ = generate_points_in_volume_checker_function(obj=self.obj, volume_link=self.link)
 
-            # Store the projection mesh's IDs
-            projection_mesh_ids = lazy.pxr.PhysicsSchemaTools.encodeSdfPath(self.projection_mesh.prim_path)
+            self.obj.states[TriggerVolumeColliders].assign_trigger_marker(self.projection_mesh)
 
             # We also generate the function for checking overlaps at runtime
             def check_overlap():
-                nonlocal valid_hit
-                valid_hit = False
-                og.sim.psqi.overlap_shape(*projection_mesh_ids, reportFn=overlap_callback)
-                return valid_hit
+                colliders = self.obj.states[TriggerVolumeColliders].get_value()
+                return any(collider not in self._link_prim_paths for collider in colliders)
 
         elif self.method == ParticleModifyMethod.ADJACENCY:
             # Define the function for checking whether points are within the adjacency mesh
@@ -676,7 +670,7 @@ def _update(self):
     @classmethod
     def get_dependencies(cls):
         deps = super().get_dependencies()
-        deps.update({AABB, Saturated, ModifiedParticles})
+        deps.update({AABB, Saturated, ModifiedParticles, TriggerVolumeColliders})
         return deps
 
     @classmethod
@@ -1092,7 +1086,7 @@ def _initialize(self):
             # metalink, and (b) zero relative orientation between the metalink and the projection mesh
             local_pos, local_quat = self.projection_mesh.get_position_orientation(frame="parent")
             assert th.all(
-                th.isclose(local_pos + th.tensor([0, 0, height / 2.0]), th.zeros_like(local_pos))
+                th.isclose(local_pos + th.tensor([0, 0, height / 2.0]), th.zeros_like(local_pos), atol=1e-5, rtol=1e-5)
             ), "Projection mesh tip should align with metalink position!"
             local_euler = T.quat2euler(local_quat)
             assert th.all(
@@ -1490,7 +1484,9 @@ def systems_to_check(self):
     @property
     def projection_is_active(self):
         # Only active if the projection mesh is enabled
-        return self.projection_emitter.GetProperty("inputs:active").Get()
+        return (
+            self.projection_emitter.GetProperty("inputs:active").Get() if self.projection_emitter is not None else False
+        )
 
     @classproperty
     def metalink_prefix(cls):

omnigibson/object_states/toggle.py

@@ -5,8 +5,8 @@
 from omnigibson.macros import create_module_macros
 from omnigibson.object_states.link_based_state_mixin import LinkBasedStateMixin
 from omnigibson.object_states.object_state_base import AbsoluteObjectState, BooleanStateMixin
+from omnigibson.object_states.trigger_volume_colliders import TriggerVolumeColliders
 from omnigibson.object_states.update_state_mixin import GlobalUpdateStateMixin, UpdateStateMixin
-from omnigibson.prims.geom_prim import VisualGeomPrim
 from omnigibson.utils.constants import PrimType
 from omnigibson.utils.numpy_utils import vtarray_to_torch
 from omnigibson.utils.python_utils import classproperty
@@ -34,11 +34,14 @@ def __init__(self, obj, scale=None):
         self.robot_can_toggle_steps = 0
         self.visual_marker = None
 
-        # We also generate the function for checking overlaps at runtime
-        self._check_overlap = None
-
         super().__init__(obj)
 
+    @classmethod
+    def get_dependencies(cls):
+        deps = super().get_dependencies()
+        deps.add(TriggerVolumeColliders)
+        return deps
+
     @classmethod
     def global_update(cls):
         # Avoid circular imports
@@ -100,7 +103,8 @@ def _initialize(self):
         # Make sure this object is not cloth
         assert self.obj.prim_type != PrimType.CLOTH, f"Cannot create ToggledOn state for cloth object {self.obj.name}!"
 
-        mesh_prim_path = f"{self.link.prim_path}/mesh_0"
+        mesh_name = "mesh_0"
+        mesh_prim_path = f"{self.link.prim_path}/{mesh_name}"
         pre_existing_mesh = lazy.omni.isaac.core.utils.prims.get_prim_at_path(mesh_prim_path)
         # Create a primitive mesh if it doesn't already exist
         if not pre_existing_mesh:
@@ -117,50 +121,27 @@ def _initialize(self):
             lazy.omni.isaac.core.utils.bounds.recompute_extents(prim=pre_existing_mesh)
             self.scale = vtarray_to_torch(pre_existing_mesh.GetAttribute("xformOp:scale").Get())
 
-        # Create the visual geom instance referencing the generated mesh prim
-        relative_prim_path = absolute_prim_path_to_scene_relative(self.obj.scene, mesh_prim_path)
-        self.visual_marker = VisualGeomPrim(
-            relative_prim_path=relative_prim_path, name=f"{self.obj.name}_visual_marker"
-        )
-        self.visual_marker.load(self.obj.scene)
+        # Make sure the object updates its meshes, and assert that there's only a single visual mesh
+        self.link.update_meshes(trigger_mesh_paths=[mesh_prim_path])
+        assert len(self.link.visual_meshes) == 1, "Toggle button must have exactly one visual mesh"
+        self.visual_marker = self.link.visual_meshes[mesh_name]
         self.visual_marker.scale = self.scale
         self.visual_marker.initialize()
         self.visual_marker.visible = True
+        # Make sure the toggle button is visible
+        self.visual_marker.purpose = "default"
 
-        # Store the projection mesh's IDs
-        projection_mesh_ids = lazy.pxr.PhysicsSchemaTools.encodeSdfPath(self.visual_marker.prim_path)
-
-        # Define function for checking overlap
-        valid_hit = False
-
-        def overlap_callback(hit):
-            nonlocal valid_hit
-            all_finger_paths = {path for path_set in self._robot_finger_paths for path in path_set}
-            valid_hit = hit.rigid_body in all_finger_paths
-            # Continue traversal only if we don't have a valid hit yet
-            return not valid_hit
-
-        # Set this value to be False by default
-        self._set_value(False)
-
-        def check_overlap():
-            nonlocal valid_hit
-            valid_hit = False
-            if self.visual_marker.prim.GetTypeName() == "Mesh":
-                og.sim.psqi.overlap_mesh(*projection_mesh_ids, reportFn=overlap_callback)
-            else:
-                og.sim.psqi.overlap_shape(*projection_mesh_ids, reportFn=overlap_callback)
-            return valid_hit
-
-        self._check_overlap = check_overlap
+        self.obj.states[TriggerVolumeColliders].assign_trigger_marker(self.visual_marker)
 
     def _update(self):
         # If we're not nearby any fingers, we automatically can't toggle
         if self.obj not in self._finger_contact_objs:
             robot_can_toggle = False
         else:
             # Check to make sure fingers are actually overlapping the toggle button mesh
-            robot_can_toggle = self._check_overlap()
+            trigger_colliders = self.obj.states[TriggerVolumeColliders].get_value()
+            all_finger_paths = {path for path_set in self._robot_finger_paths for path in path_set}
+            robot_can_toggle = bool(trigger_colliders & all_finger_paths)
 
         previous_step = self.robot_can_toggle_steps
         if robot_can_toggle:

omnigibson/object_states/trigger_volume_colliders.py

@@ -0,0 +1,21 @@
+from omnigibson.object_states.object_state_base import BooleanStateMixin, RelativeObjectState
+from omnigibson.object_states.update_state_mixin import UpdateStateMixin
+
+
+class TriggerVolumeColliders(RelativeObjectState, UpdateStateMixin):
+
+    def __init__(self, obj):
+        self.trigger_marker = None
+        self._colliding_prim_paths = []
+        super().__init__(obj)
+
+    def assign_trigger_marker(self, trigger_marker):
+        self.trigger_marker = trigger_marker
+
+    def _update(self):
+        if self.trigger_marker is None:
+            return
+        self._colliding_prim_paths = self.trigger_marker.get_colliding_prim_paths()
+
+    def _get_value(self):
+        return self._colliding_prim_paths

omnigibson/prims/geom_prim.py

@@ -444,3 +444,42 @@ def _post_load(self):
         # The purpose should be default, not guide as set by CollisionGeomPrim
         # this is to make sure the geom is visualized, even though it's also collidable
         self.purpose = "default"
+
+
+class TriggerGeomPrim(CollisionGeomPrim):
+    def __init__(
+        self,
+        relative_prim_path,
+        name,
+        load_config=None,
+    ):
+        # Store values created at runtime
+        self._trigger_api = None
+        self._trigger_state_api = None
+
+        # Run super method
+        super().__init__(
+            relative_prim_path=relative_prim_path,
+            name=name,
+            load_config=load_config,
+        )
+
+    def _post_load(self):
+        # run super first
+        super()._post_load()
+
+        # Create API references
+        self._trigger_api = (
+            lazy.pxr.PhysxSchema.PhysxTriggerAPI(self._prim)
+            if self._prim.HasAPI(lazy.pxr.PhysxSchema.PhysxTriggerAPI)
+            else lazy.pxr.PhysxSchema.PhysxTriggerAPI.Apply(self._prim)
+        )
+        self._trigger_state_api = (
+            lazy.pxr.PhysxSchema.PhysxTriggerStateAPI(self._prim)
+            if self._prim.HasAPI(lazy.pxr.PhysxSchema.PhysxTriggerStateAPI)
+            else lazy.pxr.PhysxSchema.PhysxTriggerStateAPI.Apply(self._prim)
+        )
+
+    def get_colliding_prim_paths(self):
+        targets = self._trigger_state_api.GetTriggeredCollisionsRel().GetTargets()
+        return {str(path).rsplit("/collisions", 1)[0] for path in targets}

omnigibson/prims/rigid_prim.py

@@ -9,7 +9,7 @@
 import omnigibson.lazy as lazy
 import omnigibson.utils.transform_utils as T
 from omnigibson.macros import create_module_macros, gm
-from omnigibson.prims.geom_prim import CollisionGeomPrim, VisualGeomPrim
+from omnigibson.prims.geom_prim import CollisionGeomPrim, TriggerGeomPrim, VisualGeomPrim
 from omnigibson.prims.xform_prim import XFormPrim
 from omnigibson.utils.constants import GEOM_TYPES
 from omnigibson.utils.sim_utils import CsRawData
@@ -175,7 +175,7 @@ def remove(self):
         # Then self
         super().remove()
 
-    def update_meshes(self):
+    def update_meshes(self, trigger_mesh_paths=[]):
         """
         Helper function to refresh owned visual and collision meshes. Useful for synchronizing internal data if
         additional bodies are added manually
@@ -193,9 +193,10 @@ def update_meshes(self):
                 mesh_name, mesh_path = prim.GetName(), prim.GetPrimPath().__str__()
                 mesh_prim = lazy.omni.isaac.core.utils.prims.get_prim_at_path(prim_path=mesh_path)
                 is_collision = mesh_prim.HasAPI(lazy.pxr.UsdPhysics.CollisionAPI)
+                is_trigger = mesh_path in trigger_mesh_paths
                 mesh_kwargs = {
                     "relative_prim_path": absolute_prim_path_to_scene_relative(self.scene, mesh_path),
-                    "name": f"{self._name}:{'collision' if is_collision else 'visual'}_{mesh_name}",
+                    "name": f"{self._name}:{'collision' if is_collision and not is_trigger else 'visual'}_{mesh_name}",
                 }
                 if is_collision:
                     mesh = CollisionGeomPrim(**mesh_kwargs)
@@ -217,7 +218,9 @@ def update_meshes(self):
                         log.warning(f"Got overly oblong collision mesh: {mesh.name}; use boundingCube approximation")
                         mesh.set_collision_approximation("boundingCube")
                 else:
-                    self._visual_meshes[mesh_name] = VisualGeomPrim(**mesh_kwargs)
+                    self._visual_meshes[mesh_name] = (
+                        TriggerGeomPrim(**mesh_kwargs) if is_trigger else VisualGeomPrim(**mesh_kwargs)
+                    )
                     self._visual_meshes[mesh_name].load(self.scene)
 
         # If we have any collision meshes, we aggregate their center of mass and volume values to set the center of mass

omnigibson/prims/xform_prim.py

@@ -236,9 +236,6 @@ def set_position_orientation(
             xformable_prim = lazy.usdrt.Rt.Xformable(
                 lazy.omni.isaac.core.utils.prims.get_prim_at_path(self.prim_path, fabric=True)
             )
-            assert (
-                not xformable_prim.HasWorldXform()
-            ), "Fabric's world pose is set for a non-rigid prim which is unexpected. Please report this."
             xformable_prim.SetLocalXformFromUsd()
 
     def get_position_orientation(self, frame: Literal["world", "scene", "parent"] = "world", clone=True):

tests/test_object_states.py

@@ -914,17 +914,22 @@ def test_particle_remover(env):
     # Test projection
 
     place_obj_on_floor_plane(breakfast_table)
-    place_objA_on_objB_bbox(vacuum, breakfast_table, z_offset=0.02)
     for _ in range(3):
         og.sim.step()
 
-    assert not vacuum.states[ToggledOn].get_value()
     water_system = env.scene.get_system("water")
     # Place single particle of water on middle of table
     water_system.generate_particles(
-        positions=[[0, 0, breakfast_table.aabb[1][2].item() + water_system.particle_radius]]
+        positions=[[0, 0, breakfast_table.aabb[1][2].item() + water_system.particle_radius + 0.01]]
     )
     assert water_system.n_particles > 0
+    for _ in range(3):
+        og.sim.step()
+
+    place_objA_on_objB_bbox(vacuum, breakfast_table, z_offset=0.012)
+    for _ in range(3):
+        og.sim.step()
+    assert not vacuum.states[ToggledOn].get_value()
 
     # Take number of steps for water to be removed, make sure there is still water
     n_remover_steps = vacuum.states[ParticleRemover].n_steps_per_modification